卡尔曼滤波从应用到推导 -- kalman学习笔记

一、应用：估计小车的运动状态，以我们的底盘为例。

1. 数学模型：

2. 实现效果（Code runing）：

二、原理：流程和反馈图，参数的意义。

1. 线性模型假设

2. 卡尔曼滤波工作原理图：

三、推导：五条方程式的推导过程。

1. 非线性过程的卡尔曼滤波器

2. 线性过程的卡尔曼滤波器

总共五个方程，每个方程的推导如下：

①

这个是预测模型，线性方程模型，人为建立的。

②

这个是 预测值和真实值之间误差协方差矩阵。

其中，

③

估计值方程。根据反馈思想构造的一个表达式。这一方程也可利用最大似然的思想推导得到。

如果H矩阵是常数，这也是“互补滤波”的表达式。

④

这个是卡尔曼增益。如何求解“三”中的系数K？

既然这是“ ③ ”中构造函数的系数，目的是让估计值与真实值最接近，可以用均方差最小来评判。这里用到了最小二乘的思想。这也就是为什么说它 “最优化自回归数据处理算法”。

在进行下面的推导时，请注意区分哪个真实值，哪个是估计值，并且注意量测方程的表达式：

备注：

A、K与P-、H、R相关，P-跟A、P、Q相关，也就是说K跟模型系数、噪声分布、协方差都相关；而模型系数一般为已知量，协方差是迭代结果，因此K跟噪声分布的关系相当大。合理变化Q和R能让估计效果更好。

B、协方差的对角线元素为各个变量的方差。

C、以下是对矩阵转置的理解:

⑤

这个是 估计值和真实值之间的误差协方差矩阵。

当卡尔曼增益K求出来后，即可求出这个协方差，参见“④”。

如下：

为了能够运行并且不断的递推下去，我们 只需设定最初的Pk（真值与估计值的协方差）。

四、扩展内容：经验型卡尔曼滤波。

http://blog.youkuaiyun.com/haishaoli/article/details/52965457

五、参考内容：

1. 卡尔曼滤波 -- 从推导到应用

http://blog.youkuaiyun.com/heyijia0327/article/details/17487467

http://blog.youkuaiyun.com/heyijia0327/article/details/17667341

2.《kalman卡尔曼滤波器中文介绍》

作者：李少海。联系方式：QQ395217658。