在线观看www成人影院-在线观看www日本免费网站-在线观看www视频-在线观看操-欧美18在线-欧美1级

0
  • 聊天消息
  • 系統(tǒng)消息
  • 評論與回復(fù)
登錄后你可以
  • 下載海量資料
  • 學(xué)習(xí)在線課程
  • 觀看技術(shù)視頻
  • 寫文章/發(fā)帖/加入社區(qū)
會員中心
創(chuàng)作中心

完善資料讓更多小伙伴認(rèn)識你,還能領(lǐng)取20積分哦,立即完善>

3天內(nèi)不再提示

基于卡爾曼濾波的定位算法

賽目科技 ? 來源:賽目科技 ? 2023-04-21 14:02 ? 次閱讀
加入交流群
微信小助手二維碼

掃碼添加小助手

加入工程師交流群

若無法得到車輛在地圖上的準(zhǔn)確位置及周圍障礙物的位置,那么自動駕駛便無從談起。因此在自動駕駛領(lǐng)域,對車輛自身及周圍物體的精確定位至關(guān)重要。然而現(xiàn)實環(huán)境中存在各類噪聲,無論是激光雷達(dá)、慣性導(dǎo)航器件甚至衛(wèi)星定位系統(tǒng)都無法得到完全真實的車輛位置信息。為了應(yīng)對此類情況,便需要采用濾波算法來過濾觀測噪聲,以得到更加精確的定位結(jié)果。

前言

1

何為卡爾曼濾波

在各類濾波定位算法中,卡爾曼濾波是最為知名的一種。為了解決阿波羅登月計劃中的航天器定位及噪聲干擾問題,這種算法最初在1963年由匈牙利數(shù)學(xué)家魯?shù)婪颉た柭岢鯷1]。卡爾曼濾波不僅將過去類似的基于系統(tǒng)可觀性與可控性的控制方法做出了系統(tǒng)化的整理,并且在嚴(yán)格的數(shù)學(xué)意義上證明這種方法在線性系統(tǒng)的高斯隨機(jī)噪聲過濾方法中具有最優(yōu)性。

卡爾曼濾波的簡單舉例

為了更好的理解卡爾曼濾波的基本原理,本文首先對卡爾曼濾波的原理進(jìn)行舉例說明。

試想一下,假設(shè)你要憑借地圖從A地前往B地,你要如何知道自己的位置呢?

首先,你可以通過自己的腳程判斷,“A地到B地大約10公里,憑我的腳程大約需要步行3小時左右”。

其次,還可以通過路牌和手里的地圖判斷,“這個路牌上寫著到了D鎮(zhèn),地圖上顯示D鎮(zhèn)應(yīng)該在距離B地還有3公里的地方”。

然而不論是地圖還是腳程估算都存在的較大誤差,此時我們?yōu)榱说玫礁鼫?zhǔn)確的所處位置,可以結(jié)合地圖與腳程來對當(dāng)前位置進(jìn)行估算。比如:A地到B地大約10公里,憑我的腳程大約需要步行3小時左右,目前已經(jīng)走了2小時,那么我推測再過不久就可以到達(dá)D鎮(zhèn),此時再對照地圖上D鎮(zhèn)的位置,就可以得到更加精確的位置信息,同時通過對比實際到達(dá)D鎮(zhèn)的時間,還可以反過來修正對于自己腳程的估計誤差。

在這個例子里,通過自己腳程估計的位置,在卡爾曼濾波里就被稱為狀態(tài)預(yù)測,其誤差則為估計誤差,估計方法為預(yù)測矩陣,地圖上自己的位置則為狀態(tài)觀測,從地圖換算到當(dāng)前位置的方法則被稱為觀測矩陣。卡爾曼的關(guān)鍵就在于對即將到來的觀測進(jìn)行預(yù)測,并用其對比最后實際得到的觀測,通過這兩者之間的差值進(jìn)行修正。假設(shè)用一個笛卡爾坐標(biāo)系來對這種方法進(jìn)行表示,則可參考圖1。

f174916a-dffb-11ed-bfe3-dac502259ad0.png

圖1 卡爾曼濾波原理示意圖

2

卡爾曼濾波的基本公式

根據(jù)上一節(jié)的例子,我們就可以引出卡爾曼濾波的基本公式:

(1) (2) (3) (4) (5) (6)

前兩步是根據(jù)上一個最優(yōu)估計值得出此刻的估計值和估計值的協(xié)方差,緊接著就可以得到此刻的最優(yōu)估計值和最優(yōu)估計值的協(xié)方差,然后利用此刻的最優(yōu)估計值和最優(yōu)估計值的協(xié)方差進(jìn)行下一個迭代。

公式詳解

看起來有點多,但是實際上非常緊湊,推理非常嚴(yán)謹(jǐn),接下來將對上述六個公式進(jìn)行詳細(xì)解釋。

式(1)為系統(tǒng)的狀態(tài)預(yù)測公式,通過上一時刻的位置對當(dāng)前時刻的位置進(jìn)行了推測,但是由于過程噪聲的存在,這個推測出的位置不完全符合真實情況。

式(2)為系統(tǒng)的狀態(tài)誤差傳遞,類似式(1),是基于上一時刻位置可能分布的范圍對當(dāng)前時刻的可能分布范圍做出預(yù)測,Q是可能的估計誤差。

式(3)則是當(dāng)前時刻觀測量的預(yù)測值,代表我們根據(jù)預(yù)測出的狀態(tài),推測出我們將會觀測到的讀數(shù),并非真正的讀數(shù)。

式(4)為卡爾曼增益的計算方法,其物理意義為狀態(tài)預(yù)測誤差與觀測誤差的比例關(guān)系。

式(5)則為對系統(tǒng)狀態(tài)的更新,這個公式可以分為兩方面看,后半段的代表實際讀數(shù)與推測讀數(shù)之間的差值,被稱為殘差。使用式(4)求得的卡爾曼增益與殘差相乘,可以得到對狀態(tài)預(yù)測的修正量,最終式(5)可以得到圖1中的卡爾曼位置估計更新。

式(6)則為對位置估計的分布估計,可以作為下一時刻式(2)的初值使用。

實例說明

假設(shè)車輛在進(jìn)行勻速運(yùn)動,則可以設(shè)置其遞推運(yùn)動模型為:

(7) (8)

其中式(7)代表k時刻的車輛狀態(tài),包含車輛的坐標(biāo)和在橫縱坐標(biāo)軸的速度,式(8)則為勻速運(yùn)動狀態(tài)轉(zhuǎn)換矩陣,可以通過將式(7)、(8)帶入式(1)求取下一時刻的狀態(tài)預(yù)測。

之后我們給出狀態(tài)誤差矩陣的迭代初值,通常設(shè)置為一個與狀態(tài)同階的單位陣,帶入式(2)即可求得下一時刻的誤差矩陣預(yù)測,這里Q需要根據(jù)估計模型的誤差來人為給定。

下一步,我們假設(shè)運(yùn)動系統(tǒng)使用GPS作為觀測器,經(jīng)過預(yù)處理的GPS信號可以直接輸出車輛的位置坐標(biāo),那么GPS的觀測模型可以簡化為:

(9) (10)

將式(9)與式(1)的結(jié)果帶入式(3),可以得到基于狀態(tài)預(yù)測的觀測值預(yù)測,系統(tǒng)觀測誤差的方差矩陣R可以根據(jù)傳感器的技術(shù)說明書確定。

最后,按順序?qū)⑸鲜鲎兞繋胧剑?)、(5)、(6)即可實現(xiàn)對位置坐標(biāo)的更新,同時得到的結(jié)果還可以作為下一步迭代的初值。

3

非線性卡爾曼濾波

雖然卡爾曼濾波已經(jīng)在數(shù)學(xué)上被嚴(yán)格證明具有最優(yōu)性,然而卻無法處理非線性系統(tǒng)。針對這種情況,現(xiàn)有的解決方法分為兩種,分別是基于泰勒展開進(jìn)行線性化近似的擴(kuò)展卡爾曼濾波[2]和基于多點采樣進(jìn)行均值方差近似的無跡卡爾曼濾波。

擴(kuò)展卡爾曼濾波

擴(kuò)展卡爾曼濾波通過一組式(11)所示的n階泰勒線性展開式近似表達(dá)非線性函數(shù):

(11)

如圖2所示,其階數(shù)越高,近似程度越高。

f19bc64a-dffb-11ed-bfe3-dac502259ad0.png

圖2 各級泰勒展開線性化效果

這種方法具有計算復(fù)雜度低、技術(shù)成熟的優(yōu)點,因此,目前在工程中被廣泛使用。然而由于泰勒展開式的復(fù)雜度隨著階數(shù)呈指數(shù)增長,往往在實際應(yīng)用中很難實現(xiàn)三階以上的泰勒展開,這導(dǎo)致了這種線性化方法存在較大的截斷誤差,因此限制了算法的估計精度,并且在非線性程度較高的系統(tǒng)中容易發(fā)散。

無跡卡爾曼濾波

無跡卡爾曼濾波則通過對非線性函數(shù)在目標(biāo)時刻的均值與方法進(jìn)行相似擬合來實現(xiàn)線性化近似的(如圖3所示),這種方法被稱為無跡變換(UT變換)。

f1b6ea7e-dffb-11ed-bfe3-dac502259ad0.png

圖3 無跡變換原理

UT變換已被Julier和Uhlmann[3]證明具有不低于三階泰勒展開的線性化精度,因此基于UT變換的無跡卡爾曼濾波具有相對于擴(kuò)展卡爾曼濾波更高的精度,同時其均值與方差的擬合不會受函數(shù)非線性化程度的影響,因此可以適用于更為非線性的系統(tǒng)。然而,由于使用無跡卡爾曼濾波需要設(shè)置較多參數(shù),并且計算負(fù)擔(dān)較重,因此在實際工程中仍然不能完全取代擴(kuò)展卡爾曼濾波。

基于上述原因,目前在非線性系統(tǒng)上需要根據(jù)系統(tǒng)特征與實際需求對濾波器的性能做出取舍,選擇最合適的方法進(jìn)行計算。

4

SLAM中的卡爾曼濾波與圖優(yōu)化

由于SLAM(Simultaneous Localization and Mapping,同步定位與建圖)中對于特征點、路徑與移動體的定位同卡爾曼濾波的適用范圍十分吻合,因此,早期SLAM中通常采用卡爾曼濾波在后端對輸出進(jìn)行優(yōu)化。隨著計算能力的不斷加強(qiáng),近年來基于圖優(yōu)化的SLAM方法也逐漸占據(jù)了一席之地。

從應(yīng)用角度分析,基于卡爾曼濾波和基于圖優(yōu)化的SLAM方法各有優(yōu)勢。

卡爾曼濾波具有計算速度快、系統(tǒng)簡單的優(yōu)點,但由于其對后端輸出的優(yōu)化僅基于相鄰兩個馬爾可夫鏈的信息進(jìn)行一次迭代,因此在精度上有所欠缺,更適用于高動態(tài)且對精度要求不高的場合。

相對的,因子圖優(yōu)化可以統(tǒng)籌全局信息以對各位姿輸出進(jìn)行優(yōu)化,并且優(yōu)化迭代次數(shù)可以人為設(shè)置,可以得到更加精確的后端輸出。正因如此,其每次迭代及出現(xiàn)新的輸出時都需要對全局進(jìn)行計算,使得計算負(fù)擔(dān)大幅增加。所以一般情況下,基于圖優(yōu)化的SLAM僅適用于對于精度要求極高的特殊場合。

隨著技術(shù)的發(fā)展,這兩種路線也在分別補(bǔ)齊自身的固有短板。

如基于濾波的SLAM中引入了迭代擴(kuò)展卡爾曼濾波,將最小二乘法與卡爾曼濾波相結(jié)合,對誤差而不是位姿進(jìn)行優(yōu)化,實現(xiàn)了更高的定位精度,并且可以實現(xiàn)動態(tài)設(shè)置迭代次數(shù),代表算法有LINS和Fast-Lio等。

反之,為了應(yīng)對圖優(yōu)化“牽一發(fā)而動全身”的問題,最近比較熱門的LIOM和LIO-SAM等算法引入了動態(tài)滑窗和關(guān)鍵幀機(jī)制,僅對少數(shù)相關(guān)的時刻進(jìn)行聯(lián)系,有效降低了冗余計算,并且引入了IMU預(yù)積分等技術(shù),進(jìn)一步削減系統(tǒng)的計算負(fù)擔(dān),獲得了更快的動態(tài)響應(yīng)時間。

總體來看,基于圖優(yōu)化的SLAM是未來的發(fā)展趨勢,而目前產(chǎn)品化的SLAM設(shè)備仍以濾波為主流。

審核編輯 :李倩

聲明:本文內(nèi)容及配圖由入駐作者撰寫或者入駐合作網(wǎng)站授權(quán)轉(zhuǎn)載。文章觀點僅代表作者本人,不代表電子發(fā)燒友網(wǎng)立場。文章及其配圖僅供工程師學(xué)習(xí)之用,如有內(nèi)容侵權(quán)或者其他違規(guī)問題,請聯(lián)系本站處理。 舉報投訴
  • gps
    gps
    +關(guān)注

    關(guān)注

    22

    文章

    2981

    瀏覽量

    169147
  • 算法
    +關(guān)注

    關(guān)注

    23

    文章

    4708

    瀏覽量

    95245
  • 卡爾曼濾波
    +關(guān)注

    關(guān)注

    3

    文章

    166

    瀏覽量

    25065
收藏 人收藏
加入交流群
微信小助手二維碼

掃碼添加小助手

加入工程師交流群

    評論

    相關(guān)推薦
    熱點推薦

    擴(kuò)展卡爾濾波matlab

    擴(kuò)展卡爾濾波matlabTDOA/AOA定位的擴(kuò)展卡爾
    發(fā)表于 07-14 16:01

    卡爾濾波

    卡爾濾波的估計值能很好的逼近真實值,我的疑惑是,這和濾波有什么關(guān)系,請高手介紹下卡爾
    發(fā)表于 07-04 22:57

    圖書分享:卡爾濾波算法的幾何解釋

    網(wǎng)上搜到一篇關(guān)于卡爾濾波算法的論文,對低維卡爾濾波
    發(fā)表于 06-11 15:28

    卡爾濾波算法

    已知測量值和原始值,但測量噪聲和觀測噪聲未知,如何進(jìn)行卡爾濾波。之前看了好像可以用自適應(yīng)卡爾,但不是很懂,求例子,最好有注釋的
    發(fā)表于 03-23 19:12

    LabVIEW一維卡爾濾波算法

    最近正在學(xué)習(xí)卡爾濾波算法,用LabVIEW仿照C語言寫了個一維的卡爾
    發(fā)表于 10-21 21:15

    卡爾濾波簡介

    希望這篇筆記可以幫助到你。卡爾濾波(Kalman filtering)是一種利用線性系統(tǒng)狀態(tài)方程,通過系統(tǒng)輸入輸出觀測數(shù)據(jù),對系統(tǒng)狀態(tài)進(jìn)行最優(yōu)估計的算法
    發(fā)表于 02-28 14:24

    卡爾濾波算法對比其他的濾波算法有什么優(yōu)點?

    卡爾濾波算法對比其他的濾波算法有什么優(yōu)點
    發(fā)表于 10-11 06:42

    卡爾濾波算法的改進(jìn)方案

    卡爾濾波 算法具有收斂平穩(wěn)、速度快、精度高的優(yōu)點,在信號處理中受到廣泛應(yīng)用。但由于卡爾
    發(fā)表于 06-14 16:36 ?0次下載
    <b class='flag-5'>卡爾</b><b class='flag-5'>曼</b><b class='flag-5'>濾波</b><b class='flag-5'>算法</b>的改進(jìn)方案

    基于擴(kuò)展卡爾濾波算法的無源定位效果分析

    將擴(kuò)展卡爾濾波算法由6維推廣至9維,根據(jù)觀測到的坐標(biāo)位置、方位角和俯仰角,應(yīng)用擴(kuò)展卡爾
    發(fā)表于 03-22 17:17 ?15次下載
    基于擴(kuò)展<b class='flag-5'>卡爾</b><b class='flag-5'>曼</b><b class='flag-5'>濾波</b><b class='flag-5'>算法</b>的無源<b class='flag-5'>定位</b>效果分析

    卡爾濾波算法

    卡爾濾波算法
    發(fā)表于 12-17 17:22 ?52次下載

    卡爾濾波器的基本原理

    卡爾濾波器是一種基礎(chǔ)預(yù)測定位算法。原理非常簡單易懂。
    的頭像 發(fā)表于 03-21 13:47 ?6439次閱讀

    擴(kuò)展卡爾濾波的原理

    在很多實際工程問題當(dāng)中,非線性系統(tǒng)占大多數(shù),而卡爾提出來的卡爾濾波器是一種針對線性系統(tǒng)的估計算法
    的頭像 發(fā)表于 08-12 10:06 ?6415次閱讀
    擴(kuò)展<b class='flag-5'>卡爾</b><b class='flag-5'>曼</b><b class='flag-5'>濾波</b>的原理

    淺析基于卡爾濾波定位算法

    若無法得到車輛在地圖上的準(zhǔn)確位置及周圍障礙物的位置,那么自動駕駛便無從談起。
    的頭像 發(fā)表于 04-21 14:06 ?3374次閱讀
    淺析基于<b class='flag-5'>卡爾</b><b class='flag-5'>曼</b><b class='flag-5'>濾波</b>的<b class='flag-5'>定位</b><b class='flag-5'>算法</b>

    用于定位的實用卡爾濾波

    電子發(fā)燒友網(wǎng)站提供《用于定位的實用卡爾濾波器.zip》資料免費(fèi)下載
    發(fā)表于 06-16 10:07 ?0次下載
    用于<b class='flag-5'>定位</b>的實用<b class='flag-5'>卡爾</b><b class='flag-5'>曼</b><b class='flag-5'>濾波</b>器

    卡爾濾波算法c語言實現(xiàn)方法

    卡爾濾波(Kalman Filter)是一種用于估計狀態(tài)的算法,最初由R.E. Kalman在1960年提出。它是一種線性高斯濾波器,常用
    的頭像 發(fā)表于 01-17 10:51 ?4158次閱讀
    主站蜘蛛池模板: 99久久精品99999久久 | 7m凹凸精品分类大全免费 | 末成年一级在线看片 | 欧美1819| 天堂在线观看视频观看www | 日本特黄特色大片免费看 | 性网站在线观看 | 8050午夜一级二级全黄 | 好爽好紧好大的免费视频国产 | 人人狠狠综合88综合久久 | 色人阁综合 | 欧美特黄视频在线观看 | 色综合久久久高清综合久久久 | 性欧美高清精品videos | 丁香花的视频免费观看 | 女人aaaaa片一级一毛片 | 在线种子搜索 | 49pao强力免费打造在线高清 | 日本三级在线 | 天天干天日 | 国产黄色一级网站 | 日日干干| 狠狠干夜夜骑 | 日本a级片在线播放 | 国内一级特黄女人精品毛片 | 婷婷激情狠狠综合五月 | 中国同志chinese小彬tv | 曰本女人一级毛片看一级毛 | 亚洲欧美在线一区二区 | 特黄免费 | 真人一级一级特黄高清毛片 | 免费看一级视频 | 美女自熨出白浆视频在线播放 | 福利视频免费看 | 久久综合久久久久 | 我想看一级播放片一级的 | 婷婷 夜夜 | videosxxoo18在线 | 性夜影院爽黄a爽免费看网站 | 亚洲视频区 | 天天射久久 |