Detailed explanation of Kalman filter for motion state estimation

Click on the above “ Computer vision workshop ”, choice “ Star standard ”

The dry goods arrive at the first time

Author Jia Hao （SLAM Algorithm engineer ）@ You know

Source https://zhuanlan.zhihu.com/p/395738793

Editor: amu Laboratory

Today, I will mainly record my study of robot motion state estimation , The sequence of particle filter and Kalman filter is slightly adjusted , Mainly considering the difficulty of learning and understanding , Step by step .

Then the main outline is as follows ：

1、 Kalman filtering (kalman Filter,KF) Principles and formulas

2、 Classic Kalman filter application and simple code implementation

3、 Extended Kalman filter (Extended kalman Filter EKF) principle

4、 Unscented Kalman filter (Unscented Kalman Filter,UKF) principle

As for particle filter and Monte Carlo location method , Because it overlaps with some ideas of Unscented Kalman filter , But it's another way .

1、 Kalman filter principle and formula

2、 Kalman filter case analysis and simple code implementation

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;
}

I wrote a , Verified single frame advance , Specific iterations and requirements are based on your use Ctrl+C,Ctrl+V, Well understood. , Don't go into details .

If applied to multidimensional scenarios , For example, robot posture analysis , It involves more C++ application , Often use such as Eigen,Vector Such as the library , Follow up on specific applications SLAM analysis .

3、 Extended Kalman filter （Extended kalman Filter EKF）

4、 Unscented Kalman filter (Unscented Kalman Filter,UKF)

UKF summary

UKF The advantages of , lie in ① It does not have derivatives in analytical form and complex equations ;② Its solution uses basic linear algebra , We don't even need any closed-loop form of motion or observation models , It can be understood as black box operation .

And EKF Taylor's first-order expansion calculation is different from Jacobian calculation ,UKF The cost of computing to convergence depends on sigmapoint The selection and initial state of .

The above is about KF,EKF,UKF Theoretical explanation and partial geometric understanding of , And gives KF An application reference code of , When the system is complex , Introduce more constraints to participate in the operation , It works better , Relevant code can be written according to different needs .

knowledge has no limit , Keep improving , Go further and further on the road of Algorithm Engineers , Dig deeper .

remarks ： The author is also us 「3D Vision goes from beginner to proficient 」 knowledge Special guests ： A super dry 3D Visual learning community

This article is only for academic sharing , If there is any infringement , Please contact to delete .

blockbuster ！ Computer vision workshop - Study Communication group Established

Scan the code to add a little assistant wechat , You can apply to join 3D Visual workshop - Academic paper writing and contribution   WeChat ac group , Aimed at Communication Summit 、 Top issue 、SCI、EI And so on .

meanwhile You can also apply to join our subdivided direction communication group , At present, there are mainly ORB-SLAM Series source code learning 、3D Vision 、CV& Deep learning 、SLAM、 Three dimensional reconstruction 、 Point cloud post processing 、 Autopilot 、CV introduction 、 Three dimensional measurement 、VR/AR、3D Face recognition 、 Medical imaging 、 defect detection 、 Pedestrian recognition 、 Target tracking 、 Visual products landing 、 The visual contest 、 License plate recognition 、 Hardware selection 、 Depth estimation 、 Academic exchange 、 Job exchange Wait for wechat group , Please scan the following micro signal clustering , remarks ：” Research direction + School / company + nickname “, for example ：”3D Vision  + Shanghai Jiaotong University + quietly “. Please note... According to the format , Otherwise, it will not pass . After successful addition, relevant wechat groups will be invited according to the research direction . Original contribution Please also contact .

▲ Long press and add wechat group or contribute

▲ The official account of long click attention

3D Vision goes from entry to mastery of knowledge ： in the light of 3D In the field of vision Video Course cheng （ 3D reconstruction series 、 3D point cloud series 、 Structured light series 、 Hand eye calibration 、 Camera calibration 、 laser / Vision SLAM、 Autopilot, etc ）、 Summary of knowledge points 、 Introduction advanced learning route 、 newest paper Share 、 Question answer Carry out deep cultivation in five aspects , There are also algorithm engineers from various large factories to provide technical guidance . meanwhile , The planet will be jointly released by well-known enterprises 3D Vision related algorithm development positions and project docking information , Create a set of technology and employment as one of the iron fans gathering area , near 4000 Planet members create better AI The world is making progress together , Knowledge planet portal ：

Study 3D Visual core technology , Scan to see the introduction ,3 Unconditional refund within days

  There are high quality tutorial materials in the circle 、 Can answer questions 、 Help you solve problems efficiently

Feel useful , Please give me a compliment ~