当前位置:网站首页>[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
边栏推荐
- 请查收.NET MAUI 的最新学习资源
- 竟然有一半的人不知道 for 与 foreach 的区别???
- Cmu15445 (fall 2019) project 2 - hash table details
- SwiftUI Swift 内功之 Swift 中使用不透明类型的 5 个技巧
- CMU15445 (Fall 2019) 之 Project#2 - Hash Table 详解
- 《论文阅读》Neural Approaches to Conversational AI(1)
- Half of the people don't know the difference between for and foreach???
- Table replication in PostgreSQL
- 相机标定(1): 单目相机标定及张正友标定基本原理
- 正在运行的Kubernetes集群想要调整Pod的网段地址
猜你喜欢
核舟记(一):当“男妈妈”走进现实,生物科技革命能解放女性吗?
【滤波跟踪】基于matlab捷联惯导仿真【含Matlab源码 1935期】
【系统设计】指标监控和告警系统
技术分享 | 抓包分析 TCP 协议
OneDNS助力高校行业网络安全
How much do you know about excel formula?
In my limited software testing experience, a full-time summary of automation testing experience
竟然有一半的人不知道 for 与 foreach 的区别???
Zhou Yajin, a top safety scholar of Zhejiang University, is a curiosity driven activist
Unsupervised learning of visual features by contracting cluster assignments
随机推荐
electron添加SQLite数据库
SwiftUI Swift 内功之 Swift 中使用不透明类型的 5 个技巧
【时间格式工具函数的封装】
[question] Compilation Principle
Cmu15445 (fall 2019) project 2 - hash table details
. Net Maui performance improvement
Vuthink proper installation process
Have you ever met flick Oracle CDC, read a table without update operation, and read it repeatedly every ten seconds
简单介绍一下闭包及它的一些应用场景
‘module‘ object is not callable错误
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
Complete collection of common error handling in MySQL installation
Network protocol concept
The database synchronization tool dbsync adds support for mongodb and es
Solve the problem that vscode can only open two tabs
Programming examples of stm32f1 and stm32subeide -315m super regenerative wireless remote control module drive
【纹理特征提取】基于matlab局部二值模式LBP图像纹理特征提取【含Matlab源码 1931期】
核舟记(一):当“男妈妈”走进现实,生物科技革命能解放女性吗?
R語言使用magick包的image_mosaic函數和image_flatten函數把多張圖片堆疊在一起形成堆疊組合圖像(Stack layers on top of each other)
Swiftui swift internal skill how to perform automatic trigonometric function calculation in swift