卡尔曼滤波基本原理及matlab仿真

最佳线性滤波理论起源于40年代美国科学家Wiener和前苏联科学家Ｋолмогоров等人的研究工作，后人统称为维纳滤波理论。从理论上说，维纳滤波的最大缺点是必须用到无限过去的数据，不适用于实时处理。为了克服这一缺点，60年代Kalman把状态空间模型引入滤波理论，并导出了一套递推估计算法，后人称之为卡尔曼滤波理论 。卡尔曼滤波是以最小均方误差为估计的最佳准则，来寻求一套递推估计的算法，其基本思想是：采用信号与噪声的状态空间模型，利用前一时刻地估计值和现时刻的观测值来更新对状态变量的估计，求出现时刻的估计值。它适合于实时处理和计算机运算

卡尔曼滤波器令人惊叹的地方在于，在任何时刻，他只需要上一时刻的结果。这一点非常好理解，如我们在计算均值时，有两种算法第一种为所有的元素相加求平均，也可以采用上一时刻的值通过线性组合来计算。随着数据量的增加，对第一种方法而言显然是噩梦般的，但对第二种方法并没有影响。维纳最大的缺点便是需要用到无限过去的数据。

                                             

卡尔曼滤波的五个核心公式如下所示：

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

测量的值是水位计的值，跟水位直接对应，所以H=1

迭代公式可以表示如下：

X(k|k)= X(k|k-1)+Kg(k) (Z(k)-X(k|k-1))

模拟一组测量值作为输入。假设真实水位为5m，模拟800个测量值，这些测量值的平均值为5m,实际上是高斯白噪声

   V=randn(1,N);  Y=5+V;

关于X(0|0)和P(0|0)：X会逐渐的收敛。但是对于P，一般不要取0，因为这样可能会令卡尔曼完全相信你给定的X(0|0)是系统最优的，从而使算法不能收敛。

Kg(k)= P(k|k-1) / (P(k|k-1) + R

P(k|k)=（1-Kg(k)）P(k|k-1)

<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）