matlab 卡尔曼滤波

matlab 卡尔曼滤波

clear;
close all;
clc;

n = 100;

%观测值
Z=(1:n);
%方差为1的高斯噪声、
noise = randn(1,n);

Z = Z+noise;
%Z=[1532 1487 1507 1527 1514 1455 1317 1255 1799 1791 1740 1814 1858 1891 1888 1564 1716 1803 1717 1802 1849 1859 1878 1906 1920 1909 1930 1913 1894 1883 1903 1903 1961 1941 1937 1878 1695 1343 1862 1835 1830 1836 1818 1832 1849 1839 1852 1855 1867 1932 1934 1935 1936 1942 1907 1932 1898 1944 1874 1975 1895 1875 1802 1897 1877 1878 1923 1878 1854 1850 1878 1861 1856 1858 1872 1810 1838 1903 1963 1978 2022 1465 1347 1326 1008 1084 1063 1192 1037 1039 1332  957  897  899 1051 1126 1147 1221 1148 1171 1154  769 1000 1235 1296 1403 1349 1340 1338 1192 1347 1431 1542 1409 1212 1194 1130 1125 1389 1607 1665 1484 1428 1077  974  900 931 1062 1128 1256 1270 1184 1260 1221 1223 1240 1224 1255 1216 1187 1011 1163 1130 1151 1179 1241 1219 1118 1181 1163 1094 1122 1057 1553 1403 1316 1182 1044  827  579 1223 1332  970  901  372  366  742  809 822  889 1162  982 1056 1189 1374 1071 1334 1487 1512 1338 1182 1418 1216 1466 1413 1538 1391 1233 1106 1169 1187 1097 1059 1072 1093 1164 1167 1097 1122];

%状态
X=[0;0];
%状态协方差矩阵
P=[1 0;0 1];
%状态转移矩阵
F=[1 1;0 1];

%状态转移协方差矩阵
Q=[0.0001 0;0 0.0001];
%观测矩阵
H=[1 0];

%观测噪声方差
R=1;

figure;
hold on;

X1=zeros(1,n);
X2=zeros(1,n);

for i=1:n
    disp('----------------====-------------------');
    X_ = F*X;
    disp(X_);
    P_ = F*P*F'+Q;
    disp('--');
    K = P_*H'/(H*P_*H'+R);
    disp(K);
    %disp(H);
    %disp(Z(i));
    
    HX = H*X_;
    disp(HX);
    ZH = Z(i)-HX;
    disp(ZH);
    KH = K*ZH;
    disp(K);
    disp(KH);
    disp(X_);
    X = X_+KH;
    
    %X = X_+K*(Z(i)-H*X_);
    
    
    P = (eye(2)-K*H)*P_;
    
    disp('----------');
    disp(X);
    %disp(X(1));
    %disp(X(2));
    %plot(X(1),X(2),'--k');
    
    X1(i)=X(1);
    X2(i)=X(2);

end;


plot(X1,X2);

##################