運(yùn)動(dòng)狀態(tài)估計(jì)之卡爾曼濾波詳解

點(diǎn)擊下方卡片，關(guān)注“新機(jī)器視覺”公眾號

重磅干貨，第一時(shí)間送達(dá)

作者丨佳浩（SLAM算法工程師）@知乎

來源丨h(huán)ttps://zhuanlan.zhihu.com/p/395738793

編輯丨阿木實(shí)驗(yàn)室

今天將主要記錄一下自己對機(jī)器人運(yùn)動(dòng)狀態(tài)估計(jì)的學(xué)習(xí)，粒子濾波與卡爾曼濾波的講述順序稍做調(diào)整，主要是考慮到學(xué)習(xí)理解的難度，應(yīng)該循序漸進(jìn)。

那么主要講述綱要如下：

1、卡爾曼濾波(kalman Filter，KF)原理與公式

2、經(jīng)典卡爾曼濾波應(yīng)用與簡易代碼實(shí)現(xiàn)

3、擴(kuò)展卡爾曼濾波(Extended kalman Filter EKF)原理

4、無跡卡爾曼濾波(Unscented Kalman Filter，UKF)原理

至于粒子濾波與蒙特卡羅定位方法，因?yàn)楹蜔o跡卡爾曼濾波的部分思想有重疊，但又算是另外一種方法。

1、卡爾曼濾波原理與公式

2、卡爾曼濾波的案例分析與簡易代碼實(shí)現(xiàn)

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

隨手寫了一個(gè)，驗(yàn)證過單幀推進(jìn)，具體迭代和需求根據(jù)大家使用來Ctrl+C，Ctrl+V，很好理解，不多贅述。

如果應(yīng)用于多維場景，例如機(jī)器人的姿態(tài)分析等，涉及到了更多的C++應(yīng)用，經(jīng)常使用諸如Eigen，Vector等庫，具體應(yīng)用后續(xù)跟進(jìn)SLAM分析。

3、擴(kuò)展卡爾曼濾波（Extended kalman Filter EKF）

4、無跡卡爾曼濾波(Unscented Kalman Filter，UKF)

UKF總結(jié)

UKF的優(yōu)點(diǎn)，在于①它本身不具備解析形式的導(dǎo)數(shù)和復(fù)雜的方程；②它的解算使用基本的線性代數(shù)，我們甚至不需要任何關(guān)于運(yùn)動(dòng)或觀測模型的閉環(huán)形式，可以理解為黑盒運(yùn)算。

其與EKF的泰勒一階展開計(jì)算雅克比式計(jì)算不同，UKF計(jì)算到收斂的代價(jià)取決于sigmapoint的選取與初狀態(tài)的情況。

以上便是關(guān)于KF，EKF，UKF的理論詳解與部分幾何化理解，并給出了KF的一個(gè)應(yīng)用參考代碼，當(dāng)系統(tǒng)復(fù)雜時(shí)，引入更多的約束來參加運(yùn)算，效果會(huì)更好，相關(guān)代碼可以依據(jù)不同需求嘗試編寫解決。

學(xué)無止境，精益求精，在算法工程師的路上越走越遠(yuǎn)，越挖越深。