[filter tracking] strapdown inertial navigation simulation based on MATLAB [including Matlab source code 1935]

One 、 How to get the code

How to get the code 1：
The complete code has been uploaded to my resources ：【 Filter tracking 】 be based on matlab Strapdown inertial navigation simulation 【 contain Matlab Source code 1935 period 】

How to get the code 2：
By subscribing to Ziji Shenguang blog Paid column , With proof of payment , Private Blogger , This code is available .

remarks ： Subscribe to Ziji Shenguang blog Paid column , Free access to 1 Copy code （ The period of validity From the Subscription Date , Valid for three days ）;

Two 、 Introduction to strapdown inertial navigation simulation

1 Strapdown inertial navigation system error equation
When the navigation system adopts the Northeast celestial coordinate system, the error equation of the strapdown inertial navigation system is :

δνE、δνN Is the velocity error along the East 、 North component ;φE、φN、φZ The misalignment angle is in the East 、 north 、 The component of the sky ;∇x、∇x Is the zero bias of the accelerometer ;εx、εy、εz For gyro drift ;ΩN、ΩZ For the earth to rotate in the North 、 Projection of the sky ;Ctb From computer system to navigation system ( Northeast sky coordinate system ) Transformation matrix of ;w Is the white noise vector .

Use the horizontal output of the accelerometer as the measured value , Then the measurement equation is :

z=Cx+ν

In style C=[I2O2×8], I Is the unit matrix ;ν Is the white noise vector . If you want the Kalman filter to have a satisfactory filtering effect , Need to understand the observability of the system . For any fixed position

RankVi=7Vi=[CT ATiCT (ATi)2CT (ATi)3CT (ATi)4CT (ATi)5CT (ATi)6CT (ATi)7CT (ATi)8CT (ATi)9CT]T   (3)

It can be seen that the system is unobservable , Therefore, one position alignment cannot estimate all States .

3、 ... and 、 Partial source code

%**************************************************************************
% Pure inertial navigation solution master function 
%IMU Of b-frame It's the front right bottom , The data file format is ：
%GPS Weekly seconds 、Gx、Gy、Gz、Ax、Ay、Az （G For gyro ,A Represents accelerometer ）
% Gyro and accelerometer data are in incremental form , The units are rad and m/s
% The navigation coordinates are northeast 
%**************************************************************************
clc;
clear;
close all
format long
%WGS84 Ellipsoid parameter 
WGS84.a = 6378137.0;% Long half axis 
WGS84.b=6356752.3142;% Short half shaft 
WGS84.f = 1/298.257223563;% Oblateness 
WGS84.e2=0.00669437999013;% First eccentricity squared 
WGS84.ep2=0.006739496742227;% Second eccentricity squared 
WGS84.we=7.292115e-5;% Earth rotation rate 
WGS84.GM=3.986004418e+14;% Gravity is constant 
WGS84.ge=9.7803267715;% Equatorial gravitational acceleration 
WGS84.gp=9.8321863685;% Polar gravitational acceleration 

BLH0=[23.1373950708/180*pi; 113.3713651222/180*pi;2.175 ];% initial position （ weft rad, the rad, high m）
v0=[0;0;0];% Initial speed （m/s)
Euler0(1,1)=0.0107951084511778/180*pi;% Initial Euler angle roll
Euler0(2,1)=-2.14251290749072/180*pi;%pitch
Euler0(3,1)= -75.7498049314083/180*pi;%yaw
qbn0=EulerToQuaternion(Euler0);% Initial attitude quaternion 
Cbn0=QuaternionToDCM(qbn0);% Initial direction cosine matrix 
fid=fopen('Data1.bin','r');% Open the original data file 
fp=fopen('Result.txt','w');% Open the text used to save the results 
fb=fopen('Data1_PureINS.bin','r');% Open the reference result file 
fd=fopen('error.txt','w');% Open the file that saves the difference 
fe=fopen('Data1_PureINS.txt','w');% Open the file that saves the reference results 
t0=91620.0;% Initial time 
Deltatheta0=[0;0;0];% Initial gyro output 
Deltav0=[0;0;0];% Initial accelerometer output 
button=questdlg(' Whether to draw coordinate map in real time ？（ Real time plotting is extremely slow ！）',' Tips ','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

Four 、 Running results

5、 ... and 、matlab Edition and references

1 matlab edition
2014a

2 reference
[1] Shen Zaiyang . Master MATLAB signal processing [M]. tsinghua university press ,2015.
[2] Gao Baojian , Peng Jinye , Wang Lin , Pan Jianshou . Signals and systems —— Use MATLAB Analysis and Implementation [M]. tsinghua university press ,2020.
[3] Wang Wenguang , Wei Shaoming , Ren Xin . Signal processing and system analysis MATLAB Realization [M]. Electronic industry press ,2018.
[4] Wang Yandong , Fan Yuezu . Research on multi position alignment of strapdown inertial navigation system [J]. Chinese Journal of Inertial Technology . 2000,(03)

3 remarks
This part of the introduction is taken from the Internet , For reference only , If infringement , Contact deletion