matlab 卡尔曼滤波

2021-06-13 22:02:58 阅读：195 来源： 互联网

标签：disp HX ZH 卡尔曼滤波 KH 矩阵 matlab 1878

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);

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

标签：disp,HX,ZH,卡尔曼滤波,KH,矩阵,matlab,1878
来源： https://www.cnblogs.com/herd/p/14881338.html

本站声明： 1. iCode9 技术分享网（下文简称本站）提供的所有内容，仅供技术学习、探讨和分享；
2. 关于本站的所有留言、评论、转载及引用，纯属内容发起人的个人观点，与本站观点和立场无关；
3. 关于本站的所有言论和文字，纯属内容发起人的个人观点，与本站观点和立场无关；
4. 本站文章均是网友提供，不完全保证技术分享内容的完整性、准确性、时效性、风险性和版权归属；如您发现该文章侵犯了您的权益，可联系我们第一时间进行删除；
5. 本站为非盈利性的个人网站，所有内容不会用来进行牟利，也不会利用任何形式的广告来间接获益，纯粹是为了广大技术爱好者提供技术内容和技术思想的分享性交流网站。

ICode9

matlab 卡尔曼滤波