简单卡尔曼程序详解

>> clear

N=200;%取200个数

w(1)=0;

w=randn(1,N);%产生一个1×N的行向量，第一个数为0，w为过程噪声（其和后边的v在卡尔曼理论里均为高斯白噪声）

x(1)=0;%状态x初始值

a=1;%a为状态转移阵，此程序简单起见取1

for k=2:N

    x(k)=a*x(k-1)+w(k-1);  %系统状态方程，k时刻的状态等于k-1时刻状态乘以状态转移阵加噪声（此处忽略了系统的控制量）

end

V=randn(1,N);%测量噪声

q1=std(V);      

Rvv=q1.^2;        

q2=std(x);      

Rxx=q2.^2; %此方程未用到Rxx       

q3=std(w);       

Rww=q3.^2; %Rvv、Rww分别为过程噪声和测量噪声的协方差(此方程只取一组数方差与协方差相同)       

c=0.2;

Y=c*x+V;%量测方差，c为量测矩阵，同a简化取为一个数

p(1)=0;%初始最优化估计协方差

s(1)=0;%s(1)表示为初始最优化估计

for t=2:N

    p1(t)=a.^2*p(t-1)+Rww;%p1为一步估计的协方差，此式从t-1时刻最优化估计s的协方差得到t-1时刻到t时刻一步估计的协方差

    b(t)=c*p1(t)/(c.^2*p1(t)+Rvv);%b为卡尔曼增益，其意义表示为状态误差的协方差与量测误差的协方差之比(个人见解)

    s(t)=a*s(t-1)+b(t)*(Y(t)-a*c*s(t-1));%Y(t)-a*c*s(t-1)称之为新息，是观测值与一步估计得到的观测值之差，此式由上一时刻状态的最优化估计s(t-1)得到当前时刻的最优化估计s(t)  

p(t)=p1(t)-c*b(t)*p1(t);%此式由一步估计的协方差得到此时刻最优化估计的协方差

end

t=1:N;

plot(t,s,'r',t,Y,'g',t,x,'b');%作图，红色为卡尔曼滤波，绿色为量测，蓝色为状态

%整体来说，此卡尔曼程序就是一个循环迭代的过程，给出初始的状态x和协方差p，得到下一时刻的x和p，循环带入可得到一系列的最优的状态估计值，此方法通常用于目标跟踪和定位。

>> 

>>