国产肉体XXXX裸体137大胆,国产成人久久精品流白浆,国产乱子伦视频在线观看,无码中文字幕免费一区二区三区 国产成人手机在线-午夜国产精品无套-swag国产精品-国产毛片久久国产

博客專欄

EEPW首頁 > 博客 > 手撕自動(dòng)駕駛算法—無跡卡爾曼濾波

手撕自動(dòng)駕駛算法—無跡卡爾曼濾波

發(fā)布人:計(jì)算機(jī)視覺工坊 時(shí)間:2023-07-17 來源:工程師 發(fā)布文章

1. 簡介


無損卡爾曼濾波又稱無跡卡爾曼濾波(Unscented Kalman Filter,UKF),是無損變換(Unscented Transform,UT)與標(biāo)準(zhǔn)卡爾曼濾波體系的結(jié)合,通過無損變換變換使非線性系統(tǒng)方程適用于線性假設(shè)下的標(biāo)準(zhǔn)卡爾曼體系。


UKF使用的是統(tǒng)計(jì)線性化技術(shù),我們把這種線性化的方法叫做無損變換(unscented transformation)這一技術(shù)主要通過n個(gè)在先驗(yàn)分布中采集的點(diǎn)(我們把它們叫sigma points)的線性回歸來線性化隨機(jī)變量的非線性函數(shù),由于我們考慮的是隨機(jī)變量的擴(kuò)展,所以這種線性化要比泰勒級(jí)數(shù)線性化(EKF所使用的策略)更準(zhǔn)確。


和EKF一樣,UKF也主要分為預(yù)測(cè)和更新。


UKF的基本思想是卡爾曼濾波與無損變換,它能有效地克服EKF估計(jì)精度低、穩(wěn)定性差的問題,因?yàn)椴挥煤雎愿唠A項(xiàng),所以對(duì)于非線性分布統(tǒng)計(jì)量的計(jì)算精度高。



2. CTRV運(yùn)動(dòng)模型


恒定轉(zhuǎn)率和速度模型(Constant Turn Rate and Velocity,CTRV)


2.1 CTRV的目標(biāo)狀態(tài)量


圖片


圖片


2.2 CTRV的狀態(tài)轉(zhuǎn)移函數(shù)


圖片


圖片


圖片


2.3 CTRV Process Noise


圖片


圖片


圖片



3. Prediction


分為3個(gè)步驟:


  • 產(chǎn)生Sigma點(diǎn)


  • 預(yù)測(cè)Sigma點(diǎn)的下一幀狀態(tài) (類似于粒子濾波中的預(yù)測(cè),更新粒子狀態(tài))


  • 預(yù)測(cè)系統(tǒng)狀態(tài)的均值和方差(類似于粒子濾波中的加權(quán)平均)


圖片


3.1 Generate Sigma Points


圖片


通常,假定狀態(tài)的個(gè)數(shù)為 n ,我們會(huì)產(chǎn)生 2n+1 個(gè)sigma點(diǎn),其中第一個(gè)就是我們當(dāng)前狀態(tài)的均值 μ ,sigma點(diǎn)集的均值的計(jì)算公式為:


圖片


其中的 λ 是一個(gè)超參數(shù),根據(jù)公式,λ 越大, sigma點(diǎn)就越遠(yuǎn)離狀態(tài)的均值,λ 越小, sigma點(diǎn)就越靠近狀態(tài)的均值。


圖片


在我們的CTRV模型中,狀態(tài)數(shù)量 n 除了要包含5個(gè)狀態(tài)以外,還要包含處理噪聲 μa 和 μω˙,因?yàn)檫@些處理噪聲對(duì)模型也有著非線性的影響。在增加了處理噪聲的影響以后,我們的不確定性矩陣 P 就變成了:


圖片


其中,P′ 就是我們?cè)瓉淼牟淮_定性矩陣(在CTRV模型中就是一個(gè) 5×5 的矩陣),Q是處理噪聲的協(xié)方差矩陣,在CTRV模型中考慮到直線加速度核Q的形式為:


圖片


計(jì)算增廣的Sigma Points


圖片


3.2 預(yù)測(cè)sigma point


圖片


圖片


3.3 預(yù)測(cè)均值和方差


圖片


圖片


x k+1∣k是sigma點(diǎn)集中每個(gè)點(diǎn)各個(gè)狀態(tài)量的加權(quán)和, P′ 即為先驗(yàn)分布的協(xié)方差(不確定性) P k + 1 ∣ k 由每個(gè)sigma點(diǎn)的方差的加權(quán)和求得。



4. Update


4.1 Predict Measurement


將先驗(yàn)映射到測(cè)量空間然后算出均值和方差:


測(cè)量分為兩個(gè)部分,LIDAR測(cè)量和RADAR測(cè)量,其中LIDAR測(cè)量模型本身就是線性的,所以我們重點(diǎn)還是放在RADAR測(cè)量模型的處理上面,RADAR的測(cè)量f非線性映射函數(shù)為:


圖片


Measurement model如圖所示:


圖片


再一次,我們使用無損轉(zhuǎn)換來解決,但是這里,我們可以不用再產(chǎn)生sigma points了,我們可以直接使用預(yù)測(cè)出來的sigma點(diǎn)集,并且可以忽略掉處理噪聲部分。那么對(duì)先驗(yàn)的非線性映射就可以表示為如下的sigma point預(yù)測(cè)(即預(yù)測(cè)非線性變換以后的均值和協(xié)方差):


圖片


圖片


這里的 R 也是測(cè)量噪聲,在這里我們直接將測(cè)量噪聲的協(xié)方差加到測(cè)量協(xié)方差上是因?yàn)樵撛肼晫?duì)系統(tǒng)沒有非線性影響。在本例中,以RADAR的測(cè)量為例,那么測(cè)量噪聲R為:


圖片


4.2 Update State


首先計(jì)算出sigma點(diǎn)集在狀態(tài)空間和測(cè)量空間的互相關(guān)函數(shù)T k + 1 ∣ k T


計(jì)算卡爾曼增益K k + 1 ∣


更新狀態(tài),計(jì)算$x_{k+1|k+1}(其中 z k + 1 是新得到的測(cè)量,而 z k + 1 ∣ k 則是我們根據(jù)先驗(yàn)計(jì)算出來的在測(cè)量空間的測(cè)量)。


更新狀態(tài)協(xié)方差矩陣,計(jì)算P k + 1 ∣ k + 1 


圖片


版權(quán)聲明:本文為CSDN博主「令狐少俠、」的原創(chuàng)文章,遵循CC 4.0 BY-SA版權(quán)協(xié)議,轉(zhuǎn)載請(qǐng)附上原文出處鏈接及本聲明。

原文鏈接:

https://blog.csdn.net/weixin_42905141/article/details/99710297



*博客內(nèi)容為網(wǎng)友個(gè)人發(fā)布,僅代表博主個(gè)人觀點(diǎn),如有侵權(quán)請(qǐng)聯(lián)系工作人員刪除。



關(guān)鍵詞: 汽車電子

相關(guān)推薦

技術(shù)專區(qū)

關(guān)閉