關于工具箱

本文所述的代碼需要基于PSINS工具箱，工具箱的講解：

• PSINS初學指導：https://blog.csdn.net/callmeup/article/details/137087932

本文為二維平面上的定位，另有三維的：

• 【PSINS】以速度和位置作為觀測量（即6維觀測量）的組合導航濾波，EKF實現，提供可直接運行的MATLAB代碼：https://blog.csdn.net/callmeup/article/details/144404734?spm=1011.2415.3001.5331

程序簡介

代碼概述

本代碼實現了一個在二維平面上，采用擴展卡爾曼濾波（EKF） 對捷聯慣性導航系統（SINS） 與外部觀測（模擬GPS位置和DVL速度計） 進行組合導航的完整仿真流程。它使用專業的慣性導航算法工具箱 PSINS，模擬了載體的運動軌跡、IMU傳感器誤差，并通過EKF算法融合慣性解算結果與外部觀測信息，最終對濾波后的導航精度（姿態、速度、位置）進行了詳盡的評估和可視化。

核心功能與步驟

代碼的執行流程清晰地分為以下幾個階段：

1. 環境初始化與軌跡生成 (trjsimu)

   • 清理工作區，設置隨機數種子以確保結果可重復。
   • 調用 glvs 和 psinstypedef 載入PSINS工具箱的全局常量并定義系統類型。
   • 使用 trjsegment 和 trjsimu 函數生成一條復雜的模擬軌跡，包括勻速、加速、左轉協調轉彎等多種機動動作，并計算出理想的載體姿態、速度、位置(avp)和IMU數據(imu)。

2. 傳感器誤差模擬與系統初始化

   • 使用 imuerrset 定義IMU的陀螺儀和加速度計誤差參數（零偏、白噪聲）。
   • 使用 imuadderr 將上述誤差添加到理想IMU數據中，模擬真實傳感器輸出。
   • 設置初始導航參數的誤差 (avperrset)，并初始化慣性導航解算 (insinit) 和卡爾曼濾波器 (kfinit)。

3. 擴展卡爾曼濾波 (EKF) 主循環

   • 時間更新（預測）：讀取帶噪聲的IMU數據，通過 insupdate 進行慣性導航解算，推算出最新的姿態、速度和位置。同時，更新EKF的狀態轉移矩陣和預測協方差矩陣。
   • 量測更新（校正）：在固定的時間間隔（例如每秒），模擬GPS接收機提供經緯度觀測，DVL提供水平速度觀測（在真實值上添加了噪聲）。將INS解算出的位置/速度與外部觀測值進行比較，得到量測殘差，并用EKF公式校正INS的誤差狀態（包括姿態誤差、速度誤差、位置誤差以及IMU零偏估計）。最后通過 kffeedback 將誤差狀態反饋給INS，修正其導航結果。

4. 結果分析與可視化

   • 這是代碼非常豐富的部分，它繪制了大量圖表來評估EKF的性能：
     • 誤差時序圖：繪制了航向角、東向/北向速度、緯度/經度隨時間變化的誤差。
     • 軌跡對比圖：將濾波后的估計軌跡與真實軌跡進行對比，清晰展示濾波效果。
     • 累積分布函數圖 (CDF)：分析速度誤差的統計分布。
     • 位置誤差熱力圖：在二維平面上用顏色直觀顯示不同區域的定位誤差大小。
     • 誤差統計條形圖：計算并顯示位置誤差的均值、標準差、最大值和均方根誤差(RMS)。
     • 性能評估表：在圖形和命令行中輸出全面的量化指標，如MAE（平均絕對誤差）、RMSE（均方根誤差）、95%分位數等。

關鍵技術點：

• 算法：擴展卡爾曼濾波 (EKF)
• 模型：15狀態誤差模型（3個姿態誤差、3個速度誤差、3個位置誤差、3個陀螺零偏、3個加計零偏）
• 觀測值：二維平面上的速度（東向、北向）和位置（緯度、經度）

運行結果

二維軌跡圖像：

航向角誤差曲線：

速度誤差時序圖：

位置誤差時序圖：

誤差統計特性：

MATLAB代碼

部分代碼如下：

% 【PSINS】二維平面上的位置、速度為觀測的154，濾波方法為EKF
% 作者 ：matlabfilter
% 2025-08-20/Ver1
% 清空工作空間，清除命令窗口，關閉所有圖形窗口
clear; clc; close all;
rng(0); % 設置隨機數種子為0，以確保結果可重復
glvs % 調用全局變量設置
psinstypedef(153); % 設置PSINS類型ts = 0.1;       % sampling interval
avp0 = [[0;0;0]; [0;0;0]; [0;0;0]]; % 初始化avp
traj_ = [];
%% 軌跡設置
seg = trjsegment(traj_, 'init',         0);
seg = trjsegment(seg, 'uniform',      100);
seg = trjsegment(seg, 'accelerate',   10, traj_, 1);
seg = trjsegment(seg, 'uniform',      100);
seg = trjsegment(seg, 'coturnleft',   45, 2, traj_, 4);
seg = trjsegment(seg, 'uniform',      100);
seg = trjsegment(seg, 'coturnleft',   45, 2, traj_, 4);
seg = trjsegment(seg, 'uniform',      100);
seg = trjsegment(seg, 'deaccelerate', 5,  traj_, 2); %2
seg = trjsegment(seg, 'uniform',      100);
% generate, save & plot
trj = trjsimu(avp0, seg.wat, ts, 1);% 初始設置
[nn, ts, nts] = nnts(2, trj.ts); % 獲取時間序列的參數
% imuerr = imuerrset(0.03, 100, 0.001, 5); % （注釋掉的）設置IMU誤差參數
imuerr = imuerrset(8, 14, 0.18, 57); % 設置IMU誤差參數
imu = imuadderr(trj.imu, imuerr);  % 對IMU數據添加誤差
% imuplot(imu); % （注釋掉的）繪制IMU數據% 設置初始姿態誤差
davp0 = avperrset(0.1*[1; 1; 1], 0, [1; 1; 3]);%% 速度觀測EKF
ins = insinit(avpadderr(trj.avp0, davp0), ts); % 初始化慣性導航系統

完整代碼：

如有導航、定位相關的代碼定制需求，請通過下方卡片聯系作者