文章详细探讨了卡尔曼滤波公式的应用，特别是在C++代码中的实现。解释了状态估计x（mean）和协方差P（covariance）的处理，以及状态转换模型F（_motion_mat）和观测模型H（_update_mat）的作用。文章指出在预测阶段mean计算时矩阵需要转置，以符合矩阵乘法规则。同时，文章讨论了project函数、kalman_gain（K）的计算，强调了S矩阵的对称性，以及在更新阶段协方差P的计算。作者承认自己在数学基础方面的不足，并欢迎读者指正。
卡尔曼滤波公式：
以上公式，在C++代码中，状态估计x用mean表示，协方差P用covariance表示，状态转换模型F用_motion_mat表示，观测模型H用_update_mat表示
1.为什么predict里面的mean计算时要做一下转置
因为mean是用
表示的，矩阵相乘时要竖着放
2.project函数
在update之前有个project函数，得出mean1和covariance1，分别表示：
我们用大写的S表示covariance1
3.计算K（kalman_gain）
（注意，上面可两边同乘以S是因为矩阵乘法满足结合率，右边可以先算S和S的逆的乘）
这里有一个问题，S就是上面代码中的project_cov,为什么计算K的时候代码中用的S而不是S的转置，所以要证明S的转置等于S，即证明S是个对称矩阵:
再看看project函数中对R噪声矩阵的处理：
std是一个四维向量
diag就是R，diag是由std构建的对角矩阵，一个对称阵加一个对称阵，S也肯定是对称阵，S的转置等于S，所以计算K时就直接用S就行了。
然后计算new_mean方式中的疑问和第一点是一样的原因，都是公式中的x是竖着放的，但是代码中的mean是横着放的。
4.update中的协方差P问题
求协方差的原公式是这样的：
但是代码中：
以上仅为自己的思考，由于本人矩阵等数学基础薄弱，可能某些过程走了弯路，敬请批评指正
文中作为参考的一些链接：
https://blog.csdn.net/hai008007/article/details/80511422
https://zhuanlan.zhihu.com/p/497786053
https://blog.csdn.net/swniantftftftftftf/article/details/123545212
https://blog.csdn.net/wonengguwozai/article/details/125029383
扫一扫
6318
被折叠的 条评论
为什么被折叠?
到【灌水乐园】发言
