一、擷取代碼方式
擷取代碼方式1：
完整代碼已上傳我的資源：【濾波跟蹤】基于matlab捷聯慣導仿真【含Matlab源碼 1935期】
擷取代碼方式2：
通過訂閱紫極神光部落格付費專欄，憑支付憑證，私信部落客，可獲得此代碼。
備注：訂閱紫極神光部落格付費專欄，可免費獲得1份代碼（有效期為訂閱日起，三天内有效）；
二、捷聯慣導仿真簡介
1 捷聯慣導系統誤差方程
導航系采用東北天坐标系時捷聯慣導系統的誤差方程為:
δνE、δνN為速度誤差沿東、北方向的分量;φE、φN、φZ為不對準角在東、北、天方向的分量;∇x、∇x為加速度計的零偏;εx、εy、εz為陀螺漂移;ΩN、ΩZ為地球轉速在北、天方向的投影;Ctb為機體系到導航系 (東北天坐标系) 的變換陣;w為白噪聲向量。
用加速度計的水準輸出作為測量值, 則測量方程為:
z=Cx+ν
式中C=[I2O2×8], I為機關矩陣;ν為白噪聲向量。如希望卡爾曼濾波器具有滿意的濾波效果, 需了解系統的可觀測性。對任一固定位置
RankVi=7Vi=[CT ATiCT (ATi)2CT (ATi)3CT (ATi)4CT (ATi)5CT (ATi)6CT (ATi)7CT (ATi)8CT (ATi)9CT]T (3)
可見系統是不可觀測的, 是以一位置對準不能對所有的狀态進行估計。
三、部分源代碼
%**************************************************************************
%純慣導解算主函數
%IMU的b-frame是前右下，資料檔案格式為：
%GPS周秒、Gx、Gy、Gz、Ax、Ay、Az （G代表陀螺，A代表加速度計）
%陀螺和加速度計資料均為增量形式，機關分别為rad和m/s
%導航坐标為北東地
%**************************************************************************
clc;
clear;
close all
format long
%WGS84橢球參數
WGS84.a = 6378137.0;%長半軸
WGS84.b=6356752.3142;%短半軸
WGS84.f = 1/298.257223563;%扁率
WGS84.e2=0.00669437999013;%第一偏心率平方
WGS84.ep2=0.006739496742227;%第二偏心率平方
WGS84.we=7.292115e-5;%地球自轉角速率
WGS84.GM=3.986004418e+14;%地球引力為常數
WGS84.ge=9.7803267715;%赤道重力加速度
WGS84.gp=9.8321863685;%極地重力加速度
BLH0=[23.1373950708/180*pi; 113.3713651222/180*pi;2.175 ];%初始位置（緯rad，經rad，高m）
v0=[0;0;0];%初始速度（m/s)
Euler0(1,1)=0.0107951084511778/180*pi;%初始歐拉角roll
Euler0(2,1)=-2.14251290749072/180*pi;%pitch
Euler0(3,1)= -75.7498049314083/180*pi;%yaw
qbn0=EulerToQuaternion(Euler0);%初始姿态四元數
Cbn0=QuaternionToDCM(qbn0);%初始方向餘弦陣
fid=fopen('Data1.bin','r');%打開原始資料檔案
fp=fopen('Result.txt','w');%打開用來儲存結果的文本
fb=fopen('Data1_PureINS.bin','r');%打開參考結果檔案
fd=fopen('error.txt','w');%打開儲存內插補點的檔案
fe=fopen('Data1_PureINS.txt','w');%打開儲存參考結果的檔案
t0=91620.0;%初始時間
Deltatheta0=[0;0;0];%初始陀螺輸出
Deltav0=[0;0;0];%初始加速度計輸出
button=questdlg('是否實時繪坐标圖？（實時繪圖運作極慢！）','問題提示','Yes','No','No');
if strcmp(button,'Yes')==1
figure;
plot3(BLH0(1)*180/pi,BLH0(2)*180/pi,BLH0(3),'.r');
grid on; 四、運作結果
五、matlab版本及參考文獻
1 matlab版本
2014a