卡尔曼滤波器令人惊叹的地方在于,在任何时刻,他只需要上一时刻的结果。这一点非常好理解,如我们在计算均值时,有两种算法第一种为所有的元素相加求平均,也可以采用上一时刻的值通过线性组合来计算。随着数据量的增加,对第一种方法而言显然是噩梦般的,但对第二种方法并没有影响。维纳最大的缺点便是需要用到无限过去的数据。
卡尔曼滤波的五个核心公式如下所示:
X(k|k-1)=AX(k-1|k-1)+BU(k)
P(k|k)=P(k|k-1)-K(k)HP(k)=[I-K(k) H]P(k|k-1)
X(k|k)=X(k|k-1)+K(k)(Y(k)-HX(k|k-1))
P(k|k-1)=AP(k-1|k-1)+BU(k)
K(k) = P(k|k-1)HT[ HP(k|k-1)HT +R ]-1
其中X(k|k-1)为利用系统的估计值,x(k|k)为系统得到的最优值,P(k|k-1)和P(k|k)分别为这两个数的协方差矩阵,K(k)为卡尔曼增益系数,Y(k)为系统输出值。
我们要研究的对象是一个水箱的水位,根据经验判断,这个水箱的水位是恒定的。但是会存在偏差,我们把这些偏差看成是高斯白噪声(WhiteGaussian Noise),另外,我们在房间里放一个水位计,但是这个水位计也不准确的,测量值会比实际值偏差。我们也把这些偏差看成是高斯白噪声。
对于某一分钟我们有两个有关于该水箱的水位:根据经验的预测值(系统的预测值)和水位计的值(测量值)。
下面我们要用这两个值结合他们各自的噪声来估算出房间的实际水位值。把水箱看成一个系统,对这个系统建模。当前水位和 前一时刻的水位相同的,所以A=1。没有控制量,所以U(k)=0。因此得出:
X(k|k-1)=X(k-1|k-1)+R
P(k|k-1)=P(k-1|k-1)+Q
V=randn(1,N); Y=5+V;
关于X(0|0)和P(0|0):X会逐渐的收敛。但是对于P,一般不要取0,因为这样可能会令卡尔曼完全相信你给定的X(0|0)是系统最优的,从而使算法不能收敛。
<pre name="code" class="cpp">clear
N=800;
Q=0.01;
R=2;
w(1)=0;
w=randn(1,N)*sqrt(Q);
a=1;
V(1)=0;
V=randn(1,N)*sqrt(R);%观测的协方差
c=1;
h=1;
p(1)=5;
x1(1)=2;%估计值
s(1)=2;%真值
x(1)=3
for t=2:N;
Y(t)=5+V(t);
x(t)=a*s(t-1);%估计值
p1(t)=a.^2*p(t-1)+Q; % p1(t)=P(k|k-1) p(t)=P(k|k)
K(t)=h*p1(t)/(h.^2*p1(t)+R); % 增益 R是方差
s(t)=x(t-1)+K(t)*(Y(t)-a*h*x(t-1)); % 最优 期望
p(t)=p1(t)-c*K(t)*p1(t); % 求P(k|k)
end
t=1:N;
plot(t,s,'r',t,Y,'g',t,5,'k');
axis([0 N 0 9])
figure;
h1 = stem(t -1.5, p, 'b');
hold on;
h2 = stem(t -1.25, p1, 'g');
set(h1, 'MarkerFaceColor', 'white');
set(h2, 'MarkerFaceColor', 'g');
hold off
legend([h1(1) h2(1)], 'A priori', 'A posteriori');
title('A priori and a posteriori covariance');
ylabel('Covariance');
figure;
h1 = stem(t - 1.25, K, 'b');
legend([h1(1)], 'Kalman gain');
title('Kalman gain');
ylabel('Kalman gain k');
%Set limits the same as first graph
set(gca, 'XLim', xlim);
图一为系统输出,图二为卡尔曼增益,图3为系统P(k)