卡尔曼滤波含义理解与公式推导(原作者王一哲，兰景公司，景贤计划)

waightman

已于 2022-08-30 11:37:32 修改

阅读量280

点赞数 1

文章标签： matlab 矩阵概率论

于 2022-08-27 13:29:14 首次发布

本文链接：https://blog.csdn.net/weixin_40876842/article/details/116003938

版权

卡尔曼滤波是数据融合处理以及现代信号处理中十分重要，但是读书的时候在我电过于注重形式，对其内涵认识不足，工作之后才发现廖桂生老师在矩阵分析的课程上说的太对了，其实对于数学的定义的理解以及公式背后的含义才是应用数学的精髓。在这里我们通过简单的标量卡尔曼滤波的例子和二维矢量卡尔曼滤波的例子来理解卡尔曼滤波的工作流程，将整个流程与推导过程进行对应，最后给出简单的matlab实现。
这里通过标量与简单的矢量（2维）的卡尔曼滤波的例子，理解卡尔曼滤波的原理与流程

1.1 标量卡尔曼滤波

以一维运动的无人机为例，假设无人机速度均值为2m/s，速度方差为0.4m/s,我们只用位置表示无人机的状态，即只对无人机位置进行估计。整个问题属于标量卡尔曼滤波。

请看下图：t=k-1时无人机的位置服从红色的正态分布。假设位置在21m，位置误差为0.3m，即μ=21，σ=0.3。

（1）系统的状态方程

根据无人机的这个位置，我们可以根据位置与速度经验公式，也就是”系统的结构信息“，预测出t=k时刻它的位置：位置均值是21+2=23m，我们对无人机速度的不确定度为0.4m，那么此时小车位置预测误差为0.5m（0.5是这样得到的：速度与位置都是随机变量，两个随机变量的和的方差是0.5，用了正态分布的线性叠加定理，即位置和速度两正态分布的和也满足正态分布，方差是二者平方）。可以看出0.5>0.3，此时分布变“胖”了，这很好理解——因为在递推的过程中又加了一层噪声（即预测不稳定度），所以不确定度变大了。

该部分对应的其实是模型的状态方程：x(n) = a1*x(n-1)+v1(n)+b1*u(n)+v1(n)=

a1*x(n-1)+b1*2+v(n)，其中x(n)表示n时刻的位置，v1(n)表示噪声，也就是位置误差，在这里其方差=0.3m，均值为0，u(n)=2表示无人机的飞行速度，v2(n)表示速度误差，这里为0.2m，合并之后，a1=1,b1=1。

（2）系统的测量方程

无人机的位置实时用雷达进行测量，因此整个系统的测量方程可以表示为y(n) = c1*x(n)+v2(n),其中y(n)表示雷达的测量距离，c1=1，v2(n)表示雷达的测量噪声，也就是误差，在这里测量误差为0.4m

卡尔曼滤波的问题是如何使用n时刻的数据和测量数据，预测下一个时刻n+1时刻的位置。一般采用线性形式写做：

$x\widehat{}(n)=k*x\widehat{}(n-1)+g*y(n)$

均方误差为：

$P = E[(\widehat{x}(n)-x(n))^2]=E[(k*x\widehat{}(n-1)+g*y(n)-x(n))^2]$

上面的表达式中存在两个未知量k,g。当均方误差最小的时候

$\frac{\partial P}{\partial k}=0\rightarrow E[(k*x\widehat{}(n-1)+g*y(n)-x(n))x\widehat{}(n-1))]=0$ （1）

$\frac{\partial P}{\partial g}=0\rightarrow E[(k*x\widehat{}(n-1)+g*y(n)-x(n))y(n))]=0$ （2）

由表达式（1）可以得到

$E[(k*x\widehat{}(n-1)x\widehat{}(n-1)]=E[(x(n)-g*y(n))*x\widehat{}(n-1)]$ (3)

表达式左侧加上再减去 $k\widehat{x}(n-1)x(n-1)$

$E[(k*\widehat{x}(n-1)\widehat{x}(n-1)+k\widehat{x}(n-1)x(n-1)-k\widehat{x}(n-1)x(n-1)]=E[(x(n)-g*y(n))*x\widehat{}(n-1)]$ （4）

表达式可以化简为：

$E[(k*\widehat{x}(n-1)[\widehat{x}(n-1)-x(n-1)]+k\widehat{x}(n-1)x(n-1)]=E[(x(n)-g*y(n))*x\widehat{}(n-1)]$ （5）

将表达式 $e(n)=k\widehat{x}(n)-x(n)$ 和 $y(n)=c1\widehat{x}(n)-v2(n)$ 带入表达式（5）得到

$E[(k*\widehat{x}(n-1)e(n-1)+k\widehat{x}(n-1)x(n-1)]=E[(x(n)-g*(c1\widehat{x}(n)-v2(n))))*x\widehat{}(n-1)]$ (6)

化简得到

$kE[(\widehat{x}(n-1)e(n-1)+\widehat{x}(n-1)x(n-1)]=E[x(n)(1-g*c1)+g*v2(n))*x\widehat{}(n-1)]$ (7)

由正交性可以得到表达式（7）等价于下面的表达式（8）

$kE[(\widehat{x}(n-1)x(n-1)]=(1-g*c1)E[x(n)*x\widehat{}(n-1)]$

为了避免纯估计带来的偏差，我们在t=k时刻对小车的位置坐标进行一次雷达测量，当然雷达对小车距离的测量也会受到种种因素的影响，会带来测量误差，这个测量误差的均方差是0.4cm，假设此时测量值是25cm(即均值为25)。而且测量结果也复合正态分布，于是可以画出小车在t=1时的位置服从蓝色分布。

waightman

关注

1
点赞
踩
0

收藏

觉得还不错? 一键收藏
0
评论
卡尔曼滤波含义理解与公式推导(原作者王一哲，兰景公司，景贤计划)

卡尔曼滤波是数据融合处理以及现代信号处理中十分重要，但是读书的时候在我电过于注重形式，对其内涵认识不足，工作之后才发现廖桂生老师在矩阵分析的课程上说的太对了，其实对于数学的定义的理解已经公式背后的含义才是应用数学的精髓。在这里我们通过简单的标量卡尔曼滤波的例子和二维矢量卡尔曼滤波的例子来理解卡尔曼滤波的工作流程，将整个流程与推导过程进行对应，最后给出简单的matlab实现。希望初学者从头看到尾，循序渐进，具有较好数学功底者可直接看数学公式，实用主义者可直接拷贝源代码。这里通过标量与简单的矢量（2维）的卡尔
复制链接

扫一扫