一種應用于gps導航的自適應卡爾曼濾波算法
【技術領域】
[0001 ]本發明屬于GPS導航技術領域，特別涉及一種應用于GPS導航的自適應卡爾曼濾波 算法，可用于GPS導航系統中的數據處理。
【背景技術】
[0002] 隨著精準農業的迅猛發展，以先進科學技術為主要支撐的智能農業成為農業生產 中的中堅力量，在大幅度減輕農民工作強度和傷害的同時，也大大地提高了工作效益和勞 動生產率。然而，要實現農業設備的精準作業，主要依賴于其設備對農田位置的精確感知， GPS(Global positioning System)作為農田位置感知的首選方式，在現代農業生產中得到 了極其廣泛的應用。精準農業要求GPS有更高的跟蹤精度、更準的目標識別能力、更強的抗 干擾能力、更快的反應速度。因此，GPS導航需要以智能、穩健、高效的工作模式來應對這些 挑戰。
[0003] GPS是一種無線電導航定位系統，它利用導航衛星進行測時和測距，確定用戶在空 間特定坐標系中的位置。但是在GPS導航過程中包含許多誤差，使定位精度受到嚴重的影 響，并且利用傳統的方法很難消除。而GPS自適應跟蹤濾波是消除GPS定位跟蹤誤差的重要 方法，即利用特定的濾波方法消除導航過程中的各種誤差，從而達到提高GPS導航跟蹤精度 的目的。近些年來，卡爾曼濾波理論被普遍應用于動態定位數據處理中，尤其在GPS實時定 位跟蹤方面。卡爾曼濾波技術是20世紀60年代產生的一種估計技術，是一種線性最小方差 濾波估計，它只需要通過上一時刻狀態向量的最優估計和當前時刻的觀測量就可以得到現 在時刻狀態向量的最優估計值。
[0004] 從卡爾曼濾波算法誕生到現在，GPS導航定位的數據處理主要圍繞多模型的自適 應處理方法和基于新息序列的自適應處理方法等兩方面展開研究。目前，在國際刊物上發 表的較為有效的方法是X.Rong Li等學者采用圖論的思想，提出了變結構的多模型 (Variable Structrure Multiple Model，VSMM)跟蹤算法。該算法首先要建立一個模型庫， 其中包含目標可能出現的各種狀態模型，利用模型的先驗概率和轉移概率來計算每一個濾 波器的交互估計，且多個模型間的交互比較靈活。但是，此算法的性能在很大程度上依賴于 所使用的模型集，即為了提高估計性能就需要用更多的模型來匹配目標的各種運動狀態。
[0005] 另外，現代農業要求使用高精度、低成本的定位跟蹤算法。對于精準農業生產中的 機動目標來說，它們的方位、距離和徑向速度有可能發生不同程度的變化，或快或慢。目前 的單模型機動目標跟蹤算法不能做到自適應的跟蹤，而多模型機動目標跟蹤算法在跟蹤弱 機動目標時，會有很多額外的模型不僅會大大增加系統的運算量，還會降低跟蹤器的性能， 造成了資源的浪費。因此，這些算法很難直接應用在精準農業的GPS跟蹤系統中。
【發明內容】
[0006] 為了克服上述現有技術的缺點，本發明的目的在于提供一種應用于GPS導航的自 適應卡爾曼濾波算法，基于六1?(4111:〇代〖^88；^6,41〇模型，通過設計濾波跟蹤算法，實現6?3 系統中對運動目標的自適應系統。
[0007] 為了實現上述目的，本發明采用的技術方案是：
[0008] 一種應用于GPS導航的自適應卡爾曼濾波算法，包括如下步驟：
[0009] (1)設置模型參數:狀態變量的維數M，多項式的階數N，且滿足Μ 2 N+2，狀態噪聲方 差初始矩陣Q〇，并對目標狀態的估計均值xk-i | k-dP協方差Ph |k-i進行初始化，k表示離散時 間的采樣點；初始狀態估計XQ|Q=[Z() z-1…z-Μ+ι]τ，其中Zi(i = -M+l，-M+2，'"，-l，0)為濾 波器起始之前各個時刻的目標位置觀測量；
[0010] (2)建立AR模型，在卡爾曼濾波框架下計算出AR模型系數，具體實施過程如下：
[0011] (2a)根據AR模型的定義，建立AR模型系數約束的表達式；
[0012] (2b)計算AR模型系數；
[0013] (2c)計算狀態轉移矩陣Fk|k-1;
[0014] (3)由步驟(2)得到Fki^的值，利用基于AR模型的卡爾曼濾波來預測目標狀態，目 標狀態包括均值和誤差協方差；
[0015] (4)利用測量數據更新目標狀態，并且計算增益矩陣、后驗估計均值以及后驗估計 誤差協方差；
[0016] (5)在線自適應地計算狀態噪聲協方差，輸出目標位置的估計均值和估計誤差協 方差，并且令k增加1，返回進行步驟2。
[0017] 本發明首先將AR模型套入到卡爾曼濾波算法中，通過最小均方誤差準則計算出最 優AR模型的系數;然后利用基于AR模型的卡爾曼濾波算法對目標狀態進行估計，并且利用 新息序列在線自適應地計算狀態噪聲的協方差，以更好地適應目標機動的大小。
[0018] 與現有技術相比，本發明具有以下優點：
[0019] (1)本發明提出了一種用于目標導航的預測模型一一AR模型，屬于一種自適應的 狀態模型。在濾波過程中，此模型不僅能滿足目標運動狀態的多項式約束，還能利用額外的 自由度濾除噪聲；
[0020] (2)本發明結合基于AR模型的卡爾曼濾波算法，利用新息序列在線計算狀態噪聲 的協方差矩陣，可以更加精確地估計目標的運動狀態。
【附圖說明】
[0021] 圖1是本發明的流程圖。
[0022] 圖2是本發明計算狀態噪聲協方差原理圖。
[0023] 圖3是本發明仿真實驗在算法參數匹配情況下各模型跟蹤效果圖。
[0024] 圖4是本發明仿真實驗在算法參數不匹配情況下各模型跟蹤效果圖。
[0025] 圖5是本發明現場測試實驗車運動的軌跡。
[0026] 圖6是本發明現場測試中兩種自適應算法在X方向的位置誤差圖。
[0027] 圖7是本發明現場測試中兩種自適應算法在Y方向的位置誤差圖。
【具體實施方式】
[0028] 下面結合附圖和實施例詳細說明本發明的實施方式。
[0029] 如圖1所示，本發明是一種應用于GPS導航的自適應卡爾曼濾波算法，包括如下步 驟：
[0030] 步驟1設置模型參數:狀態變量的維數M，多項式的階數N，且滿足M2 N+2,狀態噪聲 方差初始矩陣Q〇，并對目標狀態的估計均值χη |k-dP協方差Pk-i |k-i進行初始化，k表示離散 時間的采樣點；初始狀態估計Χ(φ=[ζο z-1…z-Μ+ι]τ，其中Zi(i=-M+l，-M+2,…，-1，0)為 濾波器起始之前各個時刻的目標位置觀測量；
[0031] 步驟2建立AR模型，套用卡爾曼濾波框架，并且計算出AR模型系數；
[0032]該步驟的具體實施過程如下：
[0033] (2a)建立AR模型，AR模型系數的約束為
[0034] Auk = b <1>
[0035]其中AR模型的系數Uk=[hi h2…1?1]1'，1^(丨=1，2，'"，]\〇為此濾波器的系數沁=
[1 0 …0]T，A為（N+1)XM維Vandermonde矩陣
[0036]
[0037] (2b)計算AR模型系數
[0038] 根據最小均方誤差準則，求得AR模型系數向量uk的最優解《!，計算表達如式〈3>:
[0039]
[0040] (2c)計算狀態轉移矩陣Fk | k-i，把(2b)得到的最優解《:代入Fk | k-i計算式中，Fk | k-i的 表達如式〈4>:
[0041]
[0042] 步驟3由步驟2得到Fkiw，利用基于AR模型的卡爾曼濾波來預測目標狀態；
[0043] (3a)濾波器預測模型
[0044] 目標在k+Ι時刻的狀態向量為：
[0045] xk+i=Fk+i|kXk+Wk <5>
[0046] 其中Xk=[Xk Xk-!…xk-Μ+1]τ，為目標k時刻之前Μ個采樣點的位置值，其中F k+1|k為 狀態轉移矩陣，Wk是狀態噪聲，即Wk=[Wk Wk-1…Wk-M+1]T，代表各時刻目標位置的隨機波 動，它是一個獨立同分布的零均值高斯序列，即w k~N(0，Qk)，假定不同時刻目標的位置波動 相互獨立，則AR模型中的狀態噪聲協方差表示為：
[0047] Qk = E[wk(wk)T] = qr A t · I <6>
[0048] 其中，qr為目標位置的狀態噪聲強度，△ t為采樣間隔，I為M維單位矩陣；
[0049] (3b)預測目標狀態包括預測均值和預測誤差：
[0050] xk|k-i = Fk|k-1 · xk-i|k-1 <7>
[0051]
[0052] 其中xk|k-1為k-1時刻目標狀態的預測均值，Pk|k-Ak-l時刻預測誤差，F k|k-i由式〈4 >給出；
[0053] 步驟4,使用測量數據更新目標狀態，并且計算增益矩陣、后驗估計均值以及后驗 估計誤差協方差；
[0054] (4a)濾波器觀測模型
[0055]目標在k時刻的位置觀測值可由
[0056] Zk = Hxk+vk 〈9>得到。其中觀測矩陣H=[l 0…0]1XM，Vk是觀測噪聲，代表各時 刻目標位置的觀測誤差，vk也是一個白的高斯隨機過程，與狀態噪聲wk相互獨立，即vk~N (0，Rk)，其中Rk為觀測噪聲的協方差矩陣，由于觀測向量^只有一個元素，因此觀測噪聲協 方差矩陣Rk也只有一個元素，表示出來的狀態模型和觀測模型都是線性的，因此卡爾曼濾 波算法能夠提供最小均方誤差意義下的最優狀態估計；
[0057] (4b)更新目標狀態
[0058] 使用測量數據更新目標狀態，包括增益矩陣、估計