卡尔曼滤波算法实例

卡尔曼滤波

卡尔曼滤波的含义是现时刻的最佳估计为在前一时刻的最佳估计的基础上根据现时刻的观测值作线性修正。卡尔曼滤波在数学上是一种线性最小方差统计估算方法，它是通过处理一系列带有误差的实际测量数据而得到物理参数的最佳估算。其实质要解决的问题是要寻找在最小均方误差下的估计值。它的特点是可以用递推的方法计算，其所需数据存储量较小，便于进行实时处理。具体来说，卡尔曼滤波就是要用预测方程和测量方程对系统状态进行估计。

举个例子

设我们要研究的对象是一个房间的温度。

    1.根据经验，温度是恒定的，即上一分钟的温度等于现在这一分钟的温度，经验即预测，但这并不是完全可信的，即存在一定的误差。我们假设成高斯白噪声。

    2.另外在房间里放一个温度计，实时检测房间的温度，这是观测值。同样地也存在误差，也是高斯白噪声。

    我们现在的目标就是，用这两个值，结合它们各自的噪声，估算出房间的实际温度。假设要估算k时刻的实际温度值，首先需要知道k-1时刻的温度值。

    1)预测值，假设k-1时刻房间温度为23度，则k时刻的温度应该也为23度（恒定）。同时该值的偏差为5（5是3的平方加上4的平方再开方，其中3为k-1时刻估算出的最优温度值的偏差3，4为对预测的偏差）。

    2)测量值，假设为25度，同时偏差为4度。

    那么实际值为多少？相信谁多一点，可用它们的协方差来判断：进而求得卡尔曼增益K；K=0.78
    则实际的温度值是：23+0.78*(25-23) = 24.56度。可以看出温度计测量的covariance较小，所以实际值比较偏向温度计。

    在进入k+1时刻估算前，需要算出k时刻最优值（24.56）的偏差。算法，5即k时刻估算所用k-1时刻23度温度值的偏差。得出的2.35即k时刻最优值的偏差（对应上面k-1时刻的3）。

    这样卡尔曼滤波器就不断得把方差递归，从而估算出最优值。而且它只保留上一刻的方差。上面的K就是卡尔曼增益（Kalman Gain）。

代码如下

% Kalman filter example of temperature measurement in Matlab implementation of Kalman filter algorithm.
% 房间当前温度真实值为24度，认为下一时刻与当前时刻温度相同，误差为0.02度（即认为连续的两个时刻最多变化0.02度）。
% 温度计的测量误差为0.5度。
% 开始时，房间温度的估计为23.5度，误差为1度。
close all;
clc;
clear;
% intial parameters
% 计算连续n_iter个时刻
n_iter = 100;
% size of array. n_iter行，1列
sz (n_iter, 1)=0;
% 温度的真实值
x = 24;
% 过程方差，反应连续两个时刻温度方差。更改查看效果
Q = 4e-4;
% 测量方差，反应温度计的测量精度。更改查看效果
R = 0.25;
% z是温度计的测量结果，在真实值的基础上加上了方差为0.25的高斯噪声。
z = x + sqrt®*randn(n_iter,1);
% 对数组进行初始化
% 对温度的后验估计。即在k时刻，结合温度计当前测量值与k-1时刻先验估计，得到的最终估计值
xhat = zeros(n_iter,1);
% 后验估计的方差
P = zeros(n_iter,1);
% 温度的先验估计。即在k-1时刻，对k时刻温度做出的估计
xhatpre = zeros(n_iter,1);
% 先验估计的方差
Ppre = zeros(n_iter,1);
% 卡尔曼增益，反应了温度计测量结果与过程模型（即当前时刻与下一时刻温度相同这一模型）的可信程度
K = zeros(n_iter,1);
% intial guesses
xhat(1) = 23.5; %温度初始估计值为23.5度
P(1) =1; % 误差方差为1

for k = 2:n_iter
% 时间更新（预测）
% 用上一时刻的最优估计值来作为对当前时刻的温度的预测
xhatpre(k) = xhat(k-1);
% 预测的方差为上一时刻温度最优估计值的方差与过程方差之和
Ppre(k) = P(k-1)+Q;
% 测量更新（校正）
% 计算卡尔曼增益
K(k) = Ppre(k)/( Ppre(k)+R );
% 结合当前时刻温度计的测量值，对上一时刻的预测进行校正，得到校正后的最优估计。该估计具有最小均方差
xhat(k) = xhatpre(k)+K(k)*(z(k)-xhatpre(k));
% 计算最终估计值的方差
P(k) = (1-K(k))*Ppre(k);
end

FontSize = 12;
LineWidth = 4;
figure();
plot(z,‘k+’); %画出温度计的测量值
hold on;
plot(xhat,‘b-’,‘LineWidth’,LineWidth) %画出最优估计值
hold on;
plot(x*ones(n_iter),‘g-’,‘LineWidth’,LineWidth); %画出真实值
legend(‘温度计的测量结果’, ‘后验估计’, ‘真实值’);
xl = xlabel(‘时间(分钟)’);
yl = ylabel(‘温度’);
set(xl,‘fontsize’,FontSize);
set(yl,‘fontsize’,FontSize);
hold off;
set(gca,‘FontSize’,FontSize);

figure();
valid_iter = 2:n_iter; % Pminus not valid at step 1
% 画出最优估计值的方差
plot(valid_iter,P(valid_iter),‘LineWidth’,LineWidth);
legend(‘后验估计的误差估计’);
xl = xlabel(‘时间(分钟)’);
yl = ylabel(‘℃^2’);
set(xl,‘fontsize’,FontSize);
set(yl,‘fontsize’,FontSize);
set(gca,‘FontSize’,FontSize);