当前位置:网站首页>[filter tracking] strapdown inertial navigation simulation based on MATLAB [including Matlab source code 1935]
[filter tracking] strapdown inertial navigation simulation based on MATLAB [including Matlab source code 1935]
2022-07-07 11:46:00 【Poseidon light】
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
边栏推荐
- Use references
- . Net Maui performance improvement
- [system design] index monitoring and alarm system
- Apprentissage comparatif non supervisé des caractéristiques visuelles par les assignations de groupes de contrôle
- QT | multiple windows share a prompt box class
- 一度辍学的数学差生,获得今年菲尔兹奖
- Leetcode - interview question 17.24 maximum submatrix
- 聊聊SOC启动(九) 为uboot 添加新的board
- 【问道】编译原理
- Table replication in PostgreSQL
猜你喜欢
Verilog design responder [with source code]
[system design] index monitoring and alarm system
如何在博客中添加Aplayer音乐播放器
STM32F1与STM32CubeIDE编程实例-315M超再生无线遥控模块驱动
浙江大学周亚金:“又破又立”的顶尖安全学者,好奇心驱动的行动派
Flet教程之 14 ListTile 基础入门(教程含源码)
Swiftui swift internal skill how to perform automatic trigonometric function calculation in swift
【滤波跟踪】基于matlab扩展卡尔曼滤波EKF和无迹卡尔曼滤波UKF比较【含Matlab源码 1933期】
对比学习之 Unsupervised Learning of Visual Features by Contrasting Cluster Assignments
Talk about SOC startup (x) kernel startup pilot knowledge
随机推荐
The road to success in R & D efficiency of 1000 person Internet companies
[extraction des caractéristiques de texture] extraction des caractéristiques de texture de l'image LBP basée sur le mode binaire local de Matlab [y compris le code source de Matlab 1931]
科普达人丨一文弄懂什么是云计算?
聊聊SOC启动(十) 内核启动先导知识
Talk about SOC startup (VI) uboot startup process II
SwiftUI Swift 内功之如何在 Swift 中进行自动三角函数计算
Drive HC based on de2115 development board_ SR04 ultrasonic ranging module [source code attached]
R language uses image of magick package_ Mosaic functions and images_ The flatten function stacks multiple pictures together to form a stack layers on top of each other
大佬们有没有人遇到过 flink oracle cdc,读取一个没有更新操作的表,隔十几秒就重复读取
Flet教程之 18 Divider 分隔符组件 基础入门(教程含源码)
MIF file format record
STM32 entry development NEC infrared protocol decoding (ultra low cost wireless transmission scheme)
The running kubernetes cluster wants to adjust the network segment address of pod
STM32入门开发 NEC红外线协议解码(超低成本无线传输方案)
Cmu15445 (fall 2019) project 2 - hash table details
0.96 inch IIC LCD driver based on stc8g1k08
Verilog realizes nixie tube display driver [with source code]
正在運行的Kubernetes集群想要調整Pod的網段地址
MySQL安装常见报错处理大全
What is high cohesion and low coupling?