一、几个数学概念:
二、卡尔曼滤波的推导
三、总结
首先要计算预测值、预测值和真实值之间误差协方差矩阵。
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