卡尔曼滤波器学习笔记（一）

卡尔曼滤波器的原理及应用

最近在学习Probablistic Robotics这本书，获益良多。以前学了概率论和随机过程之后一直觉得这些是虚的，不知道在工程上怎么用，而这本书恰恰就是讲如何把这些概率理论和方差估计应用到工程上去，更确切的说，应用到机器人上去。

应用前提
算法详细介绍
应用举例
下一步

1.应用前提

要应用kalman Filter,首先要有三个前提假设：

当前状态的概率分布必须是上一状态和将要执行的控制量的线性函数，再叠加一个高斯噪声。表达式如下：

其中和是状态变量，如果系统有多个自由度的话，它们表示状态向量。这样的话根据高斯分布
可以得到状态转移概率

其中表示上一状态的均值，表示方差。
对状态的测量必须是状态的线性函数叠加高斯噪声

其含义与上类似，不再赘述。
初始状态分布为高斯分布

2.算法详细介绍

Kalman Filter五条黄金公式：

这五条公式基本上就是Kalman Filter的主要内容了，它的本质就是通过预测结合测量来估计当前系统的状态。举个lizi，假如我们要估计一架飞行器的姿态，可以通过IMU来实时测量，但是测量值有一定的风险是不准确的，所以并不能完全依赖传感器。任何一个满足物理规律的系统应当是连续的，所以我们还可以通过上一状态来预测当前状态。Kalman Filter正是结合这两条进行状态估计，到底是相信哪一个多一点，还要根据Kt来决定，我们定义Kt为卡尔曼增益，它是根据测量和预测的协方差来计算的。

下面逐条解释：

line 2：首先通过上一状态最优值和将要施加的控制量来预测当前状态，由假设一可以得到：

因为我们只是求均值，而高斯噪声均值为0，所以可省去最后一项。
line 3：除了预测均值之外，我们还需要预测值的协方差来计算Kalman增益。

根据假设2，这条公式可以很容易得到。
line 4：准备工作完成之后，需要根据预测值的协方差，测量值和状态

的比例系数，测量值的协方差来计算Kalman增益。

具体证明需要用到假设中的高斯分布公式，因为我们只是应用，所以就不在blog中讨论啦，感兴趣的小伙伴可以看书中3.2.4节 Mathematical Derivation of the KF，里面讲的很详细，分享一下下载链接
http://download.csdn.net/detail/lizilpl/8632071。
line 5：这一行可以说是Kalman Filter 的精华了，现在我们有了对状态的预测值和协方差，同时也收集到了对状态的测量值。这时就可以通过kalman增益来计算状态估计值了。

增益越大，表明我们越相信测量值。

line 6：根据 line3 ，预测当前状态需要用到上一状态的协方差，所以我们还需要计算当前状态的协方差用于下一次迭代。它同样要根据Kalman增益来计算：

相信到这里，大家应该对kalman Filter的原理有了一个大致的了解，算法中，从初始状态开始，不断计算当前状态的均值和方差来迭代，直至系统结束。

3.应用实例

如下程序引用自百度百科
http://baike.baidu.com/link?url=g11J2Ab9SHiYaGB34hl86UxEMnaJyXwi_I5SrTrzKDYEMSynK1zO1Is0oXVZkR1yNKbtudaGws8j7NAdkLuV8q
为方便阅读，把原代码中关于作者版权的信息注释掉了，如有侵权，请联系我，我会第一时间补上。

程序描述了如下一个系统：

现在需要利用Kalman Filter来估计房间的实时温度

% 初始化参数
n_iter = 100;       %计算连续n_iter个时刻
sz = [n_iter, 1];
x = 24;             % 温度的真实值
Q = 4e-4;           % 对温度预测值的方差
R = 0.25;           % 测量方差，反应温度计的测量精度
T_start = 23.5;     %温度初始估计值
delta_start = 1;    %温度初始估计方差
z = x + sqrt(R)*randn(sz);
% z是温度计的测量结果，在真实值的基础上加上了方差为0.25的高斯噪声。
% 初始化数组
state_kalman=zeros(sz);
% 对温度的估计值。即在k时刻，结合温度计当前测量值与k-1时刻预测值，得到的最终估计值
variance_kalman=zeros(sz);         % 估计值的方差
state_pre=zeros(sz); % 对温度的预测
variance_pre=zeros(sz);    % 预测值的方差
K=zeros(sz);         % 卡尔曼增益
state_kalman(1) = T_start;   %温度估计值初始化
variance_kalman(1) =delta_start;   %估计值方差初始化
%
%开始迭代计算
for k = 2:n_iter
state_pre(k) = state_kalman(k-1);
%用上一时刻的最优估计值来作为对当前时刻的温度的预测
variance_pre(k) = variance_kalman(k-1)+Q;
%预测的方差为上一时刻温度最优估计值的方差与高斯噪声方差之和
%
%计算卡尔曼增益
K(k) = variance_pre(k)/( variance_pre(k)+R );
%
%结合当前时刻温度计的测量值，对上一时刻的预测进行校正，得到校正后的最优估计。由于是直接测量，故C为1.
state_kalman(k) = state_pre(k)+K(k)*(z(k)-state_pre(k));
variance_kalman(k) = (1-K(k))*variance_pre(k);
%计算最终估计值的方差用于下一次迭代
end
%绘图相关。。。。。
FontSize=14;
LineWidth=3;
figure();
plot(z,'k+'); %画出温度计的测量值
hold on;
plot(state_kalman,'b-','LineWidth',LineWidth) %画出最优估计值
hold on;
plot(x*ones(sz),'g-','LineWidth',LineWidth); %画出真实值
legend('温度测量值', 'Kalman估计值', '真实值');
xl=xlabel('时间(分钟)');
yl=ylabel('温度');
set(xl,'fontsize',FontSize);
set(yl,'fontsize',FontSize);
hold off;
set(gca,'FontSize',FontSize);
figure();
valid_iter = [2:n_iter]; % variance_pre not valid at step 1
plot(valid_iter,variance_kalman([valid_iter]),'LineWidth',LineWidth); %画出最优估计值的方差
legend('Kalman估计的误差估计');
xl=xlabel('时间(分钟)');
yl=ylabel('℃^2');
set(xl,'fontsize',FontSize);
set(yl,'fontsize',FontSize);
set(gca,'FontSize',FontSize);

“`
运行结果如下：

由结果可以看出来，虽然测量值噪声很大，但Kalman Filter的预测值还是逐渐逼近了真实温度，并且估计值的方差逐渐收敛！

4.下一步

如上讨论，经典的卡尔曼滤波只适用于线性且满足高斯分布的系统，但实际工程中并不是这么简单，比如飞行器在水平运动时有可能伴随着自身的自旋，这时就需要应用扩展卡尔曼滤波（EKF）来解决这种情况。等我研究完之后和大家分享。