最近学习了一些预测算法
卡尔曼滤波
具体推导可以查看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库,所以能手动求偏导就手动求偏导吧