第六章相机及其应用 6.4卡尔曼滤波

最新推荐文章于 2023-04-26 16:52:01 发布

白亿星星

最新推荐文章于 2023-04-26 16:52:01 发布

阅读量471

点赞数

分类专栏：相机

原文链接：https://blog.csdn.net/baidu_38172402/article/details/82710434

版权

相机专栏收录该内容

5 篇文章 2 订阅

订阅专栏

一、几个数学概念：

在这里插入图片描述

二、卡尔曼滤波的推导

在这里插入图片描述

三、总结

首先要计算预测值、预测值和真实值之间误差协方差矩阵。
1.状态方程和测量方程：
在这里插入图片描述

xk是状态向量，zk是测量向量，Ak是状态转移矩阵，uk是控制向量，Bk是控制矩阵，wk是系统误差（噪声），Hk是测量矩阵，vk是测量误差（噪声）。wk和vk都是高斯噪声，即
2.预测状态值和最小均方误差
有了这两个就能计算卡尔曼增益K，再然后得到估计值（修正状态值），
在这里插入图片描述
最后还要计算估计值和真实值之间的误差协方差矩阵（修正最小均方误差），为下次递推做准备。

至此，卡尔曼滤波的理论推导到此结束。

四、OpenCV中的KalmanFilter（卡尔曼滤波的应用）

编程步骤

step1：定义KalmanFilter类并初始化

    //构造KF对象

    KalmanFilter KF(DP, MP, 0);

    //初始化相关参数

    KF.transitionMatrix                         转移矩阵 A

    KF.measurementMatrix                  测量矩阵    H

    KF.processNoiseCov                     过程噪声 Q

    KF.measurementNoiseCov            测量噪声        R

    KF.errorCovPost                            最小均方误差 P

    KF.statePost                                系统初始状态 x(0) 

    Mat measurement                          定义初始测量值 z(0) 

step2：预测

    KF.predict( )                                                 //返回的是下一时刻的状态值KF.statePost (k+1) 

step3：更新

    更新measurement;                                     //注意measurement不能通过观测方程进行计算得到，要自己定义！

    更新KF   KF.correct(measurement)

最终的结果应该是更新后的statePost.

五、卡尔曼滤波中参数的选取

1.A和H

在这里插入图片描述

2.P0、Q和R

关于PQR的关系，看公式就一目了然：
在这里插入图片描述

本质：K= （Pk+Q） / （Pk+Q+R）

Q和R分别代表对预测值和测量值的置信度（反比），通过影响卡尔曼增益K的值，影响预测值和测量值的权重。越大的R代表越不相信测量值
Q代表建模误差，跟你计算机舍入误差（比如说用了浮点，那么会引入1e-7的误差项）、模型的线性化程度、离散化引入误差有关系；如果你还有输入u的话，输入的误差也会考虑到这里。
R的选取与传感器的特性有关，具体到应用中，比如GPS传感器传下来的位置p会在真值附近有个方差为±3m的噪声，那么R中对应p的那个元素就该是3。

原文链接：
https://blog.csdn.net/baidu_38172402/article/details/82710434
https://blog.csdn.net/heyijia0327/article/details/17487467
https://blog.csdn.net/u013453604/article/details/50301477