天天看点

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

一、获取代码方式

获取代码方式1:

完整代码已上传我的资源:​​​【滤波跟踪】基于matlab捷联惯导仿真【含Matlab源码 1935期】​​

获取代码方式2:

通过订阅紫极神光博客付费专栏,凭支付凭证,私信博主,可获得此代码。

备注:订阅紫极神光博客付费专栏,可免费获得1份代码(有效期为订阅日起,三天内有效);

二、捷联惯导仿真简介

1 捷联惯导系统误差方程

导航系采用东北天坐标系时捷联惯导系统的误差方程为:

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

δν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捷联惯导仿真【含Matlab源码 1935期】

五、matlab版本及参考文献

1 matlab版本

2014a

继续阅读