卡尔曼滤波KF与扩展卡尔曼滤波EKF算法

最近学习了一些预测算法

卡尔曼滤波

具体推导可以查看B站的教程：贝叶斯滤波与卡尔曼滤波第一讲 很有必要的绪论_哔哩哔哩_bilibili

建议从第一讲开始看

看上述教程花不了多少时间，可能看一个电影的时间就可以学会了

本文章不涉及任何推导过程（想看推导的可以退出了）

假设现在有一个信息为X（假设是一维的高斯分布）

Xe : X_evaluate即对X的估计值（卡尔曼滤波的结果）

Xp : X_predict即对X的预测（假设后一个状态至于前一个状态有关，且是线性关系）

Y：Y是对X的观测值（假设X的位置与观测到的Y也是一个线性关系）

预测方程：

设，F是Xp与Xelas的线性关系的系数，Q是这种预测方式的误差

观测方程：

设，H是Y与Xp的线性关系的系数，R是这种观测方式的误差

由于X是正态分布，我们只关心它的均值和方差

由卡尔曼滤波可得：

Xpnow的均值=Xelas的均值*F

Xpnow的方差=Xelas的方差*F*F+Q

设K=H*（Xpnow的方差）/ （H*H*（Xpnow的方差）+ R）

Xenow的均值=Xpnow的均值+K*（观测值Y - Xpnow的均值*H）

Xenow的方差=（1-K*H）*（Xpnow的方差）

若X是n维的向量信息，此时F，H，Q，R都是矩阵

对应的预测方程和观测方程也会改变了

那么就由矩阵形式的卡尔曼滤波：

Xpnow的均值=F*（Xelas的均值）

Xpnow的协方差矩阵=F*（Xelas的协方差矩阵）*（F的转置）+ Q

设K=（Xpnow的协方差矩阵）*（H的转置）/ （H*（Xpnow的协方差矩阵）*（H的转置）+ R）

Xenow的均值=Xpnow的均值+K*（观测值Y - H*（Xpnow的均值））

Xenow的协方差矩阵=（I - K*H）*（Xpnow的协方差矩阵）

如果调用了<Eigen/Dense>库的话，代码是很好写的

关于Eigen库在ubuntu下的安装，就是去官网下载最新版本，然后解压，把里面的Eigen文件夹和unsupported文件夹用sudo cp复制到usr/local/include即可

扩展卡尔曼滤波

还是一样的定义

只不过不是线性关系了，F矩阵和H矩阵变成了f(x)函数和h(x)函数了

但是没关系，我们可以求出雅克布矩阵，用一阶偏导来拟合f(x)和h(x)函数

所以，还是一样的式子，只不过在带入的时候略有区别

设F是f(x)的雅克布矩阵，H是h(x)的雅克布矩阵

Xpnow的均值=f（Xelas的均值）————可以直接带

Xpnow的协方差矩阵=F*（Xelas的协方差矩阵）*（F的转置）+ Q

设K=（Xpnow的协方差矩阵）*（H的转置）/ （H*（Xpnow的协方差矩阵）*（H的转置）+ R）

Xenow的均值=Xpnow的均值+K*（观测值Y - h（Xpnow的均值））

Xenow的协方差矩阵=（I - K*H）*（Xpnow的协方差矩阵）

雅克布矩阵是什么？

一个函数y=f(x)，传进去是一个n维向量，传出来是一个m维向量

那么它的雅克布矩阵就是一个m*n的一阶偏导矩阵

（来自百度百科）

 怎么用计算机求雅克布矩阵？

调用<ceres/jet.h>库，使用Jet类型进行运算，带入函数y=f(x)即可

关于jet.h的求导方法

其实就是给运算的对象加一个极小值标记  ，可以进行相加运算，两个  乘在一起则等于0

原理类似微积分发展初期的牛顿使用的求导方法，以及忽略二阶无穷小量的策略

关于ceres库在ubuntu下的安装

去官网，一个一个指令复制粘贴即可

代码：

自己写

upd2022.1.18:NX上竟然用不了ceres库，所以能手动求偏导就手动求偏导吧