本技術(shù)涉及導(dǎo)航定位，特別涉及一種基于卡爾曼濾波的導(dǎo)航定位方法及相關(guān)設(shè)備。

背景技術(shù)：

1、目前，全球?qū)Ш叫l(wèi)星系統(tǒng)（gnss，global?navigation?satellite?system）在車輛導(dǎo)航、無人駕駛等領(lǐng)域得到廣泛應(yīng)用與深入研究，但在城市峽谷、橋梁、林蔭等復(fù)雜場(chǎng)景下，gnss信號(hào)易受到遮擋、反射等作用，從而出現(xiàn)非視距信號(hào)（nlos，non-line?of?sight）、多路徑效應(yīng)等粗差，最終造成定位精度下降，可靠性不足等問題。

2、為有效解決gnss在復(fù)雜環(huán)境下的定位精度損失與可靠性不足等問題，基于因子圖優(yōu)化（fgo，factor?graph?optimization）的方法被應(yīng)用到gnss解算過程中。該方法基于極大似然估計(jì)，通過建立歷元內(nèi)觀測(cè)信息的因子，以及歷元間的相關(guān)性約束因子，實(shí)現(xiàn)對(duì)狀態(tài)參數(shù)的全局優(yōu)化。為了平衡計(jì)算效率與定位精度，基于滑動(dòng)窗口和邊緣化的策略被提出，具體而言，通過給解算歷元數(shù)目設(shè)置閾值，將超出閾值的歷元通過邊緣化的方式將其狀態(tài)、觀測(cè)信息壓縮，但如何對(duì)邊緣化因子的信息進(jìn)行壓縮存在一定的問題：過度相信邊緣化因子的信息，可能會(huì)導(dǎo)致后續(xù)的觀測(cè)信息失效，造成定位結(jié)果發(fā)散的問題；而過度忽視邊緣化因子的信息，則會(huì)使得在復(fù)雜環(huán)境下的定位結(jié)果易受粗差影響，造成定位精度下降的問題。

3、而目前已有的車輛導(dǎo)航的全球?qū)Ш叫l(wèi)星系統(tǒng)實(shí)時(shí)動(dòng)態(tài)定位（gnss-rtk，globalnavigation?satellite?system?real-time?kinematic）因子圖優(yōu)化算法中，較少考慮到因子圖的邊緣化因子，或僅以固定的先驗(yàn)權(quán)重評(píng)估因子圖優(yōu)化算法中的邊緣化因子。上述邊緣化策略在應(yīng)對(duì)車輛處于林蔭、城市峽谷等具有強(qiáng)遮擋、反射、衍射等作用的環(huán)境時(shí)，周跳、多路徑效應(yīng)等問題頻繁，而上述的固定權(quán)重的邊緣化因子顯然不能有效反映其信息質(zhì)量，甚至?xí)o結(jié)果帶來更差的定位結(jié)果，存在導(dǎo)航定位的準(zhǔn)確性低的問題。

技術(shù)實(shí)現(xiàn)思路

1、本技術(shù)提供了一種基于卡爾曼濾波的導(dǎo)航定位方法及相關(guān)設(shè)備，可以解決導(dǎo)航定位的準(zhǔn)確性低的問題。

2、第一方面，本技術(shù)實(shí)施例提供了一種基于卡爾曼濾波的導(dǎo)航定位方法，該導(dǎo)航定位方法包括：

3、獲取待定位車輛在個(gè)時(shí)刻的原始觀測(cè)數(shù)據(jù)；第個(gè)時(shí)刻為當(dāng)前時(shí)刻，原始觀測(cè)數(shù)據(jù)用于描述觀測(cè)到的待定位車輛的位置；

4、對(duì)所有原始觀測(cè)數(shù)據(jù)進(jìn)行解算，得到狀態(tài)參數(shù)和待定位車輛當(dāng)前時(shí)刻的三維坐標(biāo)；狀態(tài)參數(shù)包括多個(gè)與待定位車輛的導(dǎo)航定位相關(guān)的參數(shù)；

5、基于狀態(tài)參數(shù)和待定位車輛當(dāng)前時(shí)刻的三維坐標(biāo)構(gòu)建狀態(tài)方程和觀測(cè)方程，并對(duì)狀態(tài)方程和觀測(cè)方程進(jìn)行卡爾曼濾波遞推，得到待定位車輛的預(yù)測(cè)位置和邊緣化因子的狀態(tài)協(xié)方差矩陣；邊緣化因子為狀態(tài)參數(shù)中被邊緣化的參數(shù)；

6、基于狀態(tài)協(xié)方差矩陣對(duì)待定位車輛的預(yù)測(cè)位置進(jìn)行優(yōu)化，得到待定位車輛在當(dāng)前時(shí)刻的下一時(shí)刻的最終預(yù)測(cè)位置。

7、可選的，對(duì)所有原始觀測(cè)數(shù)據(jù)進(jìn)行解算，得到狀態(tài)參數(shù)結(jié)果和待定位車輛的三維坐標(biāo)，包括：

8、基于所有時(shí)刻的原始觀測(cè)數(shù)據(jù)，構(gòu)建包括待求解的狀態(tài)參數(shù)和待定位車輛待求解的三維坐標(biāo)的狀態(tài)向量集合；

9、基于狀態(tài)向量構(gòu)建目標(biāo)函數(shù)；

10、對(duì)目標(biāo)函數(shù)進(jìn)行求解，得到狀態(tài)參數(shù)和待定位車輛當(dāng)前時(shí)刻的三維坐標(biāo)。

11、可選的，基于所有時(shí)刻的原始觀測(cè)數(shù)據(jù)，構(gòu)建包括待求解的狀態(tài)參數(shù)和待定位車輛待求解的三維坐標(biāo)的狀態(tài)向量，包括：

12、通過公式：

13、?；

14、?；

15、構(gòu)建狀態(tài)向量集合；

16、其中，表示待定位車輛在第1個(gè)時(shí)刻的狀態(tài)向量，表示待定位車輛在第2個(gè)時(shí)刻的狀態(tài)向量，表示待定位車輛在第個(gè)時(shí)刻的狀態(tài)向量，表示待定位車輛在第個(gè)時(shí)刻的狀態(tài)向量，，表示待定位車輛在第個(gè)時(shí)刻時(shí)的橫坐標(biāo)，表示待定位車輛在第個(gè)時(shí)刻時(shí)的縱坐標(biāo)，表示待定位車輛在第個(gè)時(shí)刻時(shí)的豎坐標(biāo)，表示參考衛(wèi)星與第1個(gè)非參考衛(wèi)星以及待定位車輛與基準(zhǔn)站之間的雙差模糊度，表示參考衛(wèi)星與第個(gè)非參考衛(wèi)星以及待定位車輛與基準(zhǔn)站之間的雙差模糊度，表示參考衛(wèi)星與第個(gè)非參考衛(wèi)星以及待定位車輛與基準(zhǔn)站之間的雙差模糊度，表示共視衛(wèi)星的數(shù)量。

17、可選的，目標(biāo)函數(shù)為：

18、?；

19、其中，表示狀態(tài)向量集合的最優(yōu)估計(jì)，表示代價(jià)函數(shù)，表示協(xié)方差矩陣，表示馬氏范數(shù)，表示衛(wèi)星，表示雙差偽距因子，表示時(shí)間差分載波相位的測(cè)速因子，表示雙差載波相位因子，表示雙差模糊度約束因子，表示第個(gè)時(shí)刻，。

20、可選的，狀態(tài)方程為：

21、?；

22、其中，表示第個(gè)時(shí)刻優(yōu)化前由邊緣化因子預(yù)報(bào)的狀態(tài)向量，表示狀態(tài)轉(zhuǎn)移矩陣，表示第個(gè)時(shí)刻優(yōu)化后邊緣化因子內(nèi)的狀態(tài)向量，表示過程噪聲；

23、觀測(cè)方程為：

24、?；

25、其中，表示第個(gè)時(shí)刻的觀測(cè)向量，表示第個(gè)時(shí)刻的量測(cè)矩陣，表示第個(gè)時(shí)刻的狀態(tài)向量，表示觀測(cè)噪聲。

26、可選的，對(duì)狀態(tài)方程和觀測(cè)方程進(jìn)行卡爾曼濾波遞推，得到待定位車輛的預(yù)測(cè)位置和邊緣化因子的狀態(tài)協(xié)方差矩陣，包括：

27、基于狀態(tài)方程和觀測(cè)方程構(gòu)建第個(gè)時(shí)刻的狀態(tài)向量的最優(yōu)解表達(dá)式和邊緣化因子的狀態(tài)協(xié)方差矩陣；

28、對(duì)最優(yōu)解表達(dá)式進(jìn)行計(jì)算求解，得到第個(gè)時(shí)刻的狀態(tài)向量，將狀態(tài)向量中第個(gè)時(shí)刻待定位車輛的三維坐標(biāo)作為待定位車輛的預(yù)測(cè)位置。

29、可選的，最優(yōu)解表達(dá)式為：

30、?；

31、其中，表示第個(gè)時(shí)刻的狀態(tài)向量的代價(jià)函數(shù)，表示第個(gè)時(shí)刻的狀態(tài)向量的協(xié)方差矩陣，表示觀測(cè)噪聲的方差，表示由第個(gè)時(shí)刻的狀態(tài)向量的協(xié)方差矩陣預(yù)報(bào)的第個(gè)時(shí)刻的狀態(tài)向量的協(xié)方差矩陣：

32、?；

33、其中，表示在第個(gè)時(shí)刻的狀態(tài)向量的協(xié)方差矩陣，表示過程噪聲的方差；

34、邊緣化因子的狀態(tài)協(xié)方差矩陣為：

35、?；

36、其中，表示邊緣化因子協(xié)方差矩陣的更新矩陣，表示單位矩陣，表示第個(gè)時(shí)刻的卡爾曼增益矩陣：

37、?；

38、其中，表示矩陣的轉(zhuǎn)置操作。

39、可選的，基于邊緣化因子的狀態(tài)協(xié)方差矩陣對(duì)待定位車輛的預(yù)測(cè)位置進(jìn)行優(yōu)化，得到待定位車輛在當(dāng)前時(shí)刻的下一時(shí)刻的最終預(yù)測(cè)位置，包括：

40、計(jì)算邊緣化因子的殘差均方誤差、狀態(tài)參數(shù)中偽距的殘差均方誤差、狀態(tài)參數(shù)中載波相位的殘差均方誤差；

41、基于邊緣化因子的殘差均方誤差、偽距的殘差均方誤差、載波相位的殘差均方誤差構(gòu)建反饋調(diào)節(jié)因子，判斷是否滿足優(yōu)化條件；

42、若是，則基于反饋調(diào)節(jié)因子對(duì)邊緣化因子的狀態(tài)協(xié)方差矩陣進(jìn)行更新，得到更新后的狀態(tài)協(xié)方差矩陣，并將更新后的狀態(tài)協(xié)方差矩陣代入狀態(tài)參數(shù)中，得到更新后的狀態(tài)參數(shù)，并返回基于狀態(tài)參數(shù)和待定位車輛當(dāng)前時(shí)刻的三維坐標(biāo)構(gòu)建狀態(tài)方程和觀測(cè)方程，并對(duì)狀態(tài)方程和觀測(cè)方程進(jìn)行求解，得到待定位車輛的預(yù)測(cè)位置和邊緣化因子的狀態(tài)協(xié)方差矩陣的步驟；

43、否則，將待定位車輛的預(yù)測(cè)位置作為待定位車輛在當(dāng)前時(shí)刻的下一時(shí)刻的最終預(yù)測(cè)位置。

44、可選的，基于反饋調(diào)節(jié)因子對(duì)邊緣化因子的狀態(tài)協(xié)方差矩陣進(jìn)行更新，得到更新后的狀態(tài)協(xié)方差矩陣，包括：

45、通過公式：

46、?；

47、對(duì)邊緣化因子的狀態(tài)協(xié)方差矩陣進(jìn)行更新，得到更新后的狀態(tài)協(xié)方差矩陣；

48、其中，表示反饋調(diào)節(jié)因子，表示由第個(gè)時(shí)刻的狀態(tài)向量的協(xié)方差矩陣預(yù)報(bào)的第個(gè)時(shí)刻的狀態(tài)向量的協(xié)方差矩陣，表示單位矩陣，表示第個(gè)時(shí)刻的卡爾曼增益矩陣，表示第個(gè)時(shí)刻的量測(cè)矩陣。

49、第二方面，本技術(shù)實(shí)施例提供了一種基于卡爾曼濾波的導(dǎo)航定位裝置，包括：

50、獲取模塊，用于獲取待定位車輛在個(gè)時(shí)刻的原始觀測(cè)數(shù)據(jù)；第個(gè)時(shí)刻為當(dāng)前時(shí)刻，原始觀測(cè)數(shù)據(jù)用于描述觀測(cè)到的待定位車輛的位置；

51、解算模塊，用于對(duì)所有原始觀測(cè)數(shù)據(jù)進(jìn)行解算，得到狀態(tài)參數(shù)和待定位車輛當(dāng)前時(shí)刻的三維坐標(biāo)；狀態(tài)參數(shù)包括多個(gè)與待定位車輛的導(dǎo)航定位相關(guān)的參數(shù)；

52、構(gòu)建模塊，用于基于狀態(tài)參數(shù)和待定位車輛當(dāng)前時(shí)刻的三維坐標(biāo)構(gòu)建狀態(tài)方程和觀測(cè)方程，并對(duì)狀態(tài)方程和觀測(cè)方程進(jìn)行卡爾曼濾波遞推，得到待定位車輛的預(yù)測(cè)位置和邊緣化因子的狀態(tài)協(xié)方差矩陣；邊緣化因子為狀態(tài)參數(shù)中被邊緣化的參數(shù)；

53、優(yōu)化模塊，用于基于狀態(tài)協(xié)方差矩陣對(duì)待定位車輛的預(yù)測(cè)位置進(jìn)行優(yōu)化，得到待定位車輛在當(dāng)前時(shí)刻的下一時(shí)刻的最終預(yù)測(cè)位置。

54、第三方面，本技術(shù)實(shí)施例提供了一種終端設(shè)備，包括存儲(chǔ)器、處理器以及存儲(chǔ)在存儲(chǔ)器中并可在處理器上運(yùn)行的計(jì)算機(jī)程序，該處理器執(zhí)行上述計(jì)算機(jī)程序時(shí)實(shí)現(xiàn)上述的基于卡爾曼濾波的導(dǎo)航定位方法。

55、第四方面，本技術(shù)實(shí)施例提供了一種計(jì)算機(jī)可讀存儲(chǔ)介質(zhì)，該計(jì)算機(jī)可讀存儲(chǔ)介質(zhì)存儲(chǔ)有計(jì)算機(jī)程序，該計(jì)算機(jī)程序被處理器執(zhí)行時(shí)實(shí)現(xiàn)上述的基于卡爾曼濾波的導(dǎo)航定位方法。

56、本技術(shù)的上述方案有如下的有益效果：

57、在本技術(shù)的實(shí)施例中，通過獲取待定位車輛在個(gè)時(shí)刻的原始觀測(cè)數(shù)據(jù)，然后對(duì)所有原始觀測(cè)數(shù)據(jù)進(jìn)行解算，得到狀態(tài)參數(shù)和待定位車輛當(dāng)前時(shí)刻的三維坐標(biāo)，再基于狀態(tài)參數(shù)和待定位車輛的三維坐標(biāo)構(gòu)建狀態(tài)方程和觀測(cè)方程，并對(duì)狀態(tài)方程和觀測(cè)方程進(jìn)行卡爾曼濾波遞推，得到待定位車輛的預(yù)測(cè)位置和邊緣化因子的狀態(tài)協(xié)方差矩陣，最后基于狀態(tài)協(xié)方差矩陣對(duì)待定位車輛的預(yù)測(cè)位置進(jìn)行優(yōu)化，得到待定位車輛在當(dāng)前時(shí)刻的下一時(shí)刻的最終預(yù)測(cè)位置。其中，采用卡爾曼濾波的方式得到預(yù)測(cè)位置和邊緣化因子的狀態(tài)協(xié)方差矩陣，能夠?qū)Υㄎ卉囕v的位置進(jìn)行初步的預(yù)測(cè)，同時(shí)確定邊緣化因子，基于狀態(tài)協(xié)方差矩陣對(duì)待定位車輛的預(yù)測(cè)位置進(jìn)行優(yōu)化，能夠?qū)︻A(yù)測(cè)位置進(jìn)行反饋修正，提高預(yù)測(cè)位置的準(zhǔn)確性，進(jìn)而提高對(duì)待定位車輛的導(dǎo)航定位的準(zhǔn)確性。

58、本技術(shù)的其它有益效果將在隨后的具體實(shí)施方式部分予以詳細(xì)說明。