【滤波跟踪】基于matlab捷联惯导仿真【含Matlab源码 1935期】

一、获取代码方式

获取代码方式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;
    hold on
end

四、运行结果

五、matlab版本及参考文献

1 matlab版本
2014a

2 参考文献
[1] 沈再阳.精通MATLAB信号处理[M].清华大学出版社，2015.
[2]高宝建,彭进业,王琳,潘建寿.信号与系统——使用MATLAB分析与实现[M].清华大学出版社，2020.
[3]王文光,魏少明,任欣.信号处理与系统分析的MATLAB实现[M].电子工业出版社，2018.
[4]王艳东,范跃祖.捷联惯导系统多位置对准研究[J].中国惯性技术学报. 2000,(03)

3 备注
简介此部分摘自互联网，仅供参考，若侵权，联系删除