[發明專利]基于模糊卡爾曼濾波的北斗偽距定位算法

在審

• 申請號: 202110628632.2
• 申請日: 2021-06-07
• 公開（公告）號: CN113376672A
• 公開（公告）日: 2021-09-10
• 發明（設計）人: 曹學瑤;胡黃水
• 申請（專利權）人: 長春工業大學
• 主分類號: G01S19/42
• 分類號: G01S19/42;G01S19/40
• 代理公司: 暫無信息
• 代理人: 暫無信息

針對最小二乘無法對觀測數據中的粗差進行處理以及擴展卡爾曼濾波對初始位置敏感且量測噪聲方差不變的問題，提出一種結合抗差M估計和模糊卡爾曼濾波的定位算法來提高北斗導航的偽距定位精度。該算法首先利用抗差M估計對用戶端的初始位置進行定位解算，然后將解算出的位置坐標作為模糊卡爾曼濾波的初始值，最后通過模糊推理系統（FIS）得到的修正值在線調整模糊卡爾曼濾波的量測噪聲，進而提高北斗導航系統的定位精度。仿真結果表明所提算法相比擴展卡爾曼濾波精度大約提高了60%。

技術領域

本發明涉及一種模糊卡爾曼偽距定位算法，特別是一種基于抗差M估計的模糊卡爾曼偽距定位算法。

背景技術

北斗衛星導航系統是我國自主創建研發的衛星導航系統，不僅在交通運輸、農業林業、水力監測等領域都得到普遍應用，還可以為用戶端提供全天候、全天時的高精度定位服務。但提高北斗定位精度仍然受到很多因素的限制。如：復雜的信道環境，導致信號無法傳播；系統誤差和隨機誤差對偽距測量的干擾導致定位精度不高。

目前已經提出大量關于偽距定位的算法，如牛頓迭代法、極大似然估計、最小二乘法、卡爾曼濾波算法等。這些算法在提高北斗導航偽距定位精度上得到了廣泛的應用。

發明內容

本發明主要針對解決最小二乘無法對觀測數據中的粗差無法進行處理以及卡爾曼濾波對初始位置敏感且量測噪聲方差不變的問題，提出一種基于模糊卡爾曼濾波算法進行偽距定位。

本發明一種基于模糊卡爾曼濾波的北斗偽距定位算法算法由兩部分構成，即抗差M估計以及模糊卡爾曼濾波。抗差M估計基于Huber影響函數抑制觀測值粗差；模糊卡爾曼濾波基于擴展卡爾曼濾波加入模糊控制系統實時調節濾波增益，進而提高定位精度。

所述抗差M估計算法考慮到粗差對定位精度的影響，在最小二乘法的基礎上加入Hunber影響函數自動為含有粗差的觀測值分配小的加權值，達到抑制粗差的目的。

所述模糊卡爾曼濾波算法考慮到量測噪聲具有隨機性,但擴展卡爾曼濾波中的量測噪聲的方差從始自終都采用初始計算時的先驗值,因此引入模糊控制系統調節量測噪聲的協方差矩陣來減小濾波發散。

附圖說明

圖1是模糊邏輯控制器的隸屬度函數示意圖；

圖2是擴展卡爾曼濾波算法在X、Y、Z三個方向的誤差對比圖；

圖3是模糊卡爾曼濾波在X、Y、Z三個方向的誤差對比圖；

圖4是擴展卡爾曼濾波與模糊卡爾曼濾波運動軌跡與接收機真實軌跡對比圖。

具體實施方式

下面結合附圖對本發明作進一步詳細說明，本發明一種基于模糊卡爾曼濾波的北斗偽距定位算法由兩部分構成，即抗差M估計以及模糊卡爾曼濾波。抗差M估計基于Huber影響函數抑制觀測值粗差；模糊卡爾曼濾波基于擴展卡爾曼濾波加入模糊控制系統實時調節濾波增益，進而提高定位精度。

抗差M估計基于最二乘法實現的。在北斗導航系統中，最小二乘法是偽距定位系統的觀測方程。初始值處線性化得到的系統觀測模型為：，為m×l的矢量，其表示狀態殘差，即真實位置與初始化后得到的位置之間的差值， m為當前歷元下的衛星個數。為m×l的矢量，其表示偽距殘差即偽距的觀測值與估計值之間的差值；矩陣是一個的觀測矩陣；表示m×l的誤差矢量，其元素一般假設為相互獨立的高斯隨機過程。首先用最小二乘法解算此線性方程，目標函數為：

（1）

初始值處的最小二乘解和偽距殘差矢量為：

當測量噪聲為高斯分布時，最小二乘估計為最優估計。但北斗導航系統中觀測量中含有粗差會直接影響最小二乘定位精度，因此采用M估計抑制粗差。其目標函數為：

（4）

函數是可導偶函數。M估計可以按照不同情況將構造成相應的加權函數；是 方差的估計值，計算公式如下：

（5）