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

作者 | 佳浩  编辑 | 阿木实验室

链接地址：https://zhuanlan.zhihu.com/p/395738793

点击下方卡片，关注“自动驾驶与AI”公众号

ADAS巨卷干货，即可获取

点击进入→自动驾驶之心【目标跟踪】技术交流群

后台回复【领域综述】获取自动驾驶感知定位融合近80篇综述论文！

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

那么主要讲述纲要如下：

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的一个应用参考代码，当系统复杂时，引入更多的约束来参加运算，效果会更好，相关代码可以依据不同需求尝试编写解决。

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

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

视频课程来了！

自动驾驶之心为大家汇集了毫米波雷达视觉融合、高精地图、BEV感知、传感器标定、自动驾驶协同感知、语义分割、自动驾驶仿真、L4感知等多个方向学习视频，欢迎大家自取（扫码进入学习）

（扫码学习最新视频）

国内首个自动驾驶学习社区

近1000人的交流社区，和20+自动驾驶技术栈学习路线，想要了解更多自动驾驶感知（分类、检测、分割、关键点、车道线、3D目标检测、Occpuancy、多传感器融合、目标跟踪、光流估计、轨迹预测）、自动驾驶定位建图（SLAM、高精地图）、自动驾驶规划控制、领域技术方案、AI模型部署落地实战、行业动态、岗位发布，欢迎扫描下方二维码，加入自动驾驶之心知识星球，这是一个真正有干货的地方，与领域大佬交流入门、学习、工作、跳槽上的各类难题，日常分享论文+代码+视频，期待交流！

【自动驾驶之心】全栈技术交流群

自动驾驶之心是首个自动驾驶开发者社区，聚焦目标检测、语义分割、全景分割、实例分割、关键点检测、车道线、目标跟踪、3D目标检测、BEV感知、多传感器融合、SLAM、光流估计、深度估计、轨迹预测、高精地图、NeRF、规划控制、模型部署落地、自动驾驶仿真测试、产品经理、硬件配置、AI求职交流等方向；

添加汽车人助理微信邀请入群

备注：学校/公司+方向+昵称