运动状态估计之卡尔曼滤波详解

共 2195字,需浏览 5分钟

 ·

2022-07-12 15:37

点击下方卡片,关注“新机器视觉”公众号

重磅干货,第一时间送达

者丨佳浩(SLAM算法工程师)@知
来源丨https://zhuanlan.zhihu.com/p/395738793
编辑丨阿木实验室

今天将主要记录一下自己对机器人运动状态估计的学习,粒子滤波与卡尔曼滤波的讲述顺序稍做调整,主要是考虑到学习理解的难度,应该循序渐进。

那么主要讲述纲要如下:

1、卡尔曼滤波(kalman Filter,KF)原理与公式

2、经典卡尔曼滤波应用与简易代码实现

3、扩展卡尔曼滤波(Extended kalman Filter EKF)原理

4、无迹卡尔曼滤波(Unscented Kalman Filter,UKF)原理

至于粒子滤波与蒙特卡罗定位方法,因为和无迹卡尔曼滤波的部分思想有重叠,但又算是另外一种方法。


1、卡尔曼滤波原理与公式




2、卡尔曼滤波的案例分析与简易代码实现

int main(){    //predict para    double ori_gauss = 3;    double pre_gauss = 4;//inherent deviation    double step_gauss;    double pre_data;    //observation para    double ob_gauss = 4;//inherent deviation    double ob_data;    //kalman filter para    double gain_K;    //pre & ob process data in     pre_data = 23;    ob_data = 25;    //kalman filter process    step_gauss = pow((pow(ori_gauss,2)+pow(pre_gauss,2)),0.5);    gain_K = pow(pow(step_gauss,2)/(pow(step_gauss,2)+pow(ob_gauss,2)),0.5);    pre_data = pre_data + gain_K*(ob_data - pre_data);    ori_gauss = pow((1 - gain_K) * pow(step_gauss,2),0.5);    //data output    std::cout << "Kalman gain is : " << gain_K <<std::endl;    std::cout << "prediction is : " << pre_data <<std::endl;    std::cout << "new gauss deviation is : " << ori_gauss <<std::endl;    //data renew    pre_data = pre_data;//if have new pre then replace, or use old one but easier diffuse    ob_data = ob_data;//new ob replace    return 0;}


随手写了一个,验证过单帧推进,具体迭代和需求根据大家使用来Ctrl+C,Ctrl+V,很好理解,不多赘述。

如果应用于多维场景,例如机器人的姿态分析等,涉及到了更多的C++应用,经常使用诸如Eigen,Vector等库,具体应用后续跟进SLAM分析。


3、扩展卡尔曼滤波(Extended kalman Filter EKF)



4、无迹卡尔曼滤波(Unscented Kalman Filter,UKF)


UKF总结

UKF的优点,在于①它本身不具备解析形式的导数和复杂的方程;②它的解算使用基本的线性代数,我们甚至不需要任何关于运动或观测模型的闭环形式,可以理解为黑盒运算。

其与EKF的泰勒一阶展开计算雅克比式计算不同,UKF计算到收敛的代价取决于sigmapoint的选取与初状态的情况。





以上便是关于KF,EKF,UKF的理论详解与部分几何化理解,并给出了KF的一个应用参考代码,当系统复杂时,引入更多的约束来参加运算,效果会更好,相关代码可以依据不同需求尝试编写解决。

学无止境,精益求精,在算法工程师的路上越走越远,越挖越深。


本文仅做学术分享,如有侵权,请联系删文。

—THE END—
浏览 27
点赞
评论
收藏
分享

手机扫一扫分享

分享
举报
评论
图片
表情
推荐
点赞
评论
收藏
分享

手机扫一扫分享

分享
举报