Kalman滤波实验代码

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