浅谈卡尔曼滤波

首先给出卡尔曼滤波器的总体性概述.Kalman滤波器用反馈控制的方法估计过程状态：滤波器估计过程某一时刻的状态，然后以含噪声的测量方式获得反馈。故而，卡尔曼滤波可分为两个部分，时间更新方程和测量更新方程。时间更新方程负责及时向前推算当前状态变量和误差协方差估计的值，以便为下一个时间状态构造先验估计；测量更新方程负责反馈。总而言之，时间更新方程可视为预估方程，测量更新方程可视为校正方程，最后形成一种具有数值解的预估-校正算法。
先给出一MATLAB程序如下所示：`

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=1;
Y=c*x+V; %Y为测量值

p(1)=0; %设定测量值初值
s(1)=0; %设定卡尔曼方程的初始值为0
for t=2:N;
p1(t)=a.^2*p(t-1)+Rww; %这儿假设的A为一常数，得到系统的预计值
b(t)=c*p1(t)/(c.^2*p1(t)+Rvv); %这儿算的是根据测量值及测量协方差得到卡尔曼增益
s(t)=a*s(t-1)+b(t)*(Y(t)-c*a*s(t-1)); %卡尔曼滤波后得到的轨迹
p(t)=p1(t)-c*b(t)*p1(t); %c相当于测量中的Y=cx＋V,c为一常数，在这儿没变，实际中它是变化的
%更新每次得到的卡尔曼增益（即残余增益）K
end

t=1:N;
plot(t,s,’r’,t,Y,’g’,t,x,’b’);`

以上程序的编写莫过于利用卡尔曼滤波的5个基本方程式，利用时间更新方程：

以及滤波器状态更新方程