卡尔曼滤波简介

Posted 2022-11-27 Young_Gy

卡尔曼滤波是一种在不确定状况下组合多源信息得到所需状态最优估计的一种方法。本文将简要介绍卡尔曼滤波的原理及推导。

什么是卡尔曼滤波

首先定义问题：对于某一系统，知道当前状态$X_t$，存在以下两个问题：

1. 经过时间$\\triangle t$后，下个状态$X_t+1$如何求出？
2. 假定已求出$X_t+1$，在$t+1$时刻收到传感器的非直接信息$Z_t+1$，如何对状态$X_t+1$进行更正？

这两个问题正是卡尔曼滤波要解决的问题，形式化两个问题如下：

1. 预测未来
2. 修正当下

下面，将以机器人导航为例，从预测未来和修正当下两个角度介绍卡尔曼滤波器。

卡尔曼滤波的原理

问题场景如下：一个机器人，我们想知道它实时的状态$\\vecx$，同时也想做到预测未来和修正当下这两件事。

其状态$x$表示为一维大小为2的向量，元素分别表示位置信息与速度信息：

$$\\vecx = \\beginbmatrix p\\\\ v \\endbmatrix$$

可是状态$x$不一定是精准的，其不确定性用协方差表示：

$$P_k = \\beginbmatrix \\Sigma_pp & \\Sigma_pv \\\\ \\Sigma_vp & \\Sigma_vv \\\\ \\endbmatrix$$

预测未来

只考虑自身状态

只考虑自身状态的情况下，根据物理公式，可得：

$$\\beginsplit \\colordeeppinkp_k &= \\colorroyalbluep_k-1 + \\Delta t &\\colorroyalbluev_k-1 \\\\ \\colordeeppinkv_k &= &\\colorroyalbluev_k-1 \\endsplit$$

用矩阵表示如下：

$$\\beginalign \\colordeeppink\\mathbf\\hatx_k &= \\beginbmatrix 1 & \\Delta t \\\\ 0 & 1 \\endbmatrix \\colorroyalblue\\mathbf\\hatx_k-1 \\\\ &= \\mathbfF_k \\colorroyalblue\\mathbf\\hatx_k-1 \\endalign$$

在状态变化的过程中引入了新的不确定性，根据协方差的乘积公式可得：

$$\\beginequation \\beginsplit Cov(x) &= \\Sigma\\\\ Cov(\\colorfirebrick\\mathbfAx) &= \\colorfirebrick\\mathbfA \\Sigma \\colorfirebrick\\mathbfA^T \\endsplit \\labelcovident \\endequation$$

x̂ kPk以上是关于卡尔曼滤波简介的主要内容，如果未能解决你的问题，请参考以下文章

如何使用卡尔曼滤波器预测测量之间的 gps 位置

在扩展卡尔曼滤波器中计算状态空间模型和测量模型的雅可比矩阵

C语言实现简单卡尔曼滤波

卡尔曼滤波器中预测矩阵和测量协方差矩阵之间的混淆

关于卡尔曼滤波

卡尔曼滤波器基本应用/学习 - 代码似乎很慢