clear N=200; w(1)=0; w=randn(1,N) x(1)=0; a=1; for k=2:N; x(k)=a*x(k-1)+w(k-1);%求当前时刻的估计值 end V=randn(1,N); q1=std(V); Rvv=q1.^2; q2=std(x); Rxx=q2.^2; q3=std(w); Rww=q3.^2; c=0.2; Y=c*x+V;%测量方程,其中V为测量系统的噪声,c为测量系统的参数 p(1)=0; s(1)=0; for t=2:N; p1(t)=a.^2*p(t-1)+Rww;%求当前时刻的估计值的偏差,a为系统参数,没有控制量,所以没有参数b,Rww为噪声 b(t)=c*p1(t)/(c.^2*p1(t)+Rvv);%求Kg,b(t)为Kg,即Kalman增量 s(t)=a*s(t-1)+b(t)*(Y(t)-a*c*s(t-1));%求t时刻的最优值,即当前时刻的最优值 p(t)=p1(t)-c*b(t)*p1(t);%求当前最状态最优值的偏差,即式子5:(1-c*b(t))*p1(t) end t=1:N; p