写在前面的话：文章内容来源于但不限于网络、书籍、个人心得体会等，意在总结和方便各位同行快速参考，共同学习进步，记录自己的问题。错误在所难免，有请各位批评斧正。如有侵权，烦请第一时间通知，我会立即删除相关内容，万分感谢！
clear; N = 100; s = [N, 1]; p=[1,0,1,0]; T=1; g=[1,T,0,0;0,1,0,0;0,0,T,1;0,0,0,1]; Q=[0.5*T^2,0;T,0;0,0.5*T^2;0,T]; t=5e2; R=0.02^2; x=zeros(4,t); w=mvnrnd([0,0],diag(0.1^2*ones(1,2)),t)'; v=mvnrnd(0,0.02^2,t)'; z = p*x +v; x(k+1)=g*x(k)+Q*w state_kalman=zeros(s); variance_kalman=w; variance_pre=v; K=zeros(s); state_kalman(1) = z; for k = 2:N state_pre(k) = state_kalman(k-1); variance_pre(k) = variance_kalman(k-1)+Q; K(k) = variance_pre(k)/( variance_pre(k)+R ); state_kalman(k) = state_pre(k)+K(k)*(z(k)-state_pre(k)); variance_kalman(k) = (1-K(k))*variance_pre(k); end FontSize=14; LineWidth=3; figure(); plot(z,'m+'); hold on; plot(state_kalman,'b-','LineWidth',LineWidth) hold on; plot(x,'g-','LineWidth',LineWidth); legend('测量值', 'Kalman估计值');
