惯导解算步骤
1.初始对准得到载体的姿态角后，由欧拉角得到四元数初始值：
其中q=a+bi+cj+dk，[a,b,c,d]也即[q0,q1,q2,q3]。
2.由四元数初始值构造姿态转移矩阵n
c——载体坐标系b系到导航
b
坐标系（也即地理坐标系）n系的转移矩阵
其中，俯仰角θ，滚转角φ，航向角ψ
3.由n b C求出e b C
n b e n e b C C C =
其中：
其中，ϕ和λ分别为纬度和经度。
4. 由e
b C 反求e 系下四元数的初始值
[a,b,c,d]也即[q0,q1,q2,q3]。
5. 由四元数按时间传递的特性：
也即：
⎥⎥
⎥⎥⎦
⎤
⎢⎢⎢⎢⎣⎡⎥⎥⎥⎥⎥⎦⎤⎢⎢⎢⎢
⎢
⎣⎡------=⎥
⎥⎥⎥⎦⎤⎢⎢⎢⎢⎣⎡32103210000021q q q q q q q q x
y
z
x z
y
y z
x z y
x ωωωωωωωωωωωω （2-24） 利用毕卡逼近法求解可得：
()()()*1
2
0M dt q t e q t ω⎰
= （2-25）
令：
[]()*0000x y z x
z y y z x z
y
x
M dt θθθθ
θθθωθθθθθθ-∆-∆-∆⎡⎤
⎢⎥∆∆-∆⎢
⎥∆==⎢⎥
∆-∆∆⎢⎥∆∆-∆⎢⎥⎣⎦
⎰ （2-26） （2-25）可简写为：
()[]()1
2
0q t e q θ∆= （2-27）
将[]1
2
e
θ∆展开可得：
()[][][][]()23
1111022!23!2!2n
q t I q n θθθθ⎡⎤
⎛⎫⎛⎫⎛⎫
∆∆∆⎢⎥=+∆+++
++ ⎪ ⎪ ⎪⎢⎥⎝⎭⎝⎭⎝⎭
⎣⎦
（2-28） 由于：
[][][]
[]2
24
2
222
03
04
0x y z I I
I
θθθθθθθθθθ⎡⎤∆==-∆+∆+∆=-∆⎣⎦∆=-∆∆∆=-∆ （2-29） 将式（2-29）代入式（2-28）整理可得：
[]000sin 2()cos (0)2q t I q θθθθ∆⎧⎫⎪⎪∆=+∆⎨⎬∆⎪⎪⎩⎭
（2-30） 在实际解算中，把0cos 2θ∆和0sin 2
θ
∆展为级数形式并取有限项，得四元
数的各阶近似算法。
一阶算法：
[]1
(1)()2
q n I q n θ⎧⎫
+=+∆⎨⎬⎩
⎭
（2-31）
二阶算法：
()[]2
01(1)1()82q n I q n θθ⎧⎫⎛⎫∆⎪⎪
+=-+∆ ⎪⎨⎬ ⎪⎪⎪⎝⎭⎩⎭
（2-32）
若取一阶算法则有：
其中，角增量△θ：
其中的b e R 即b e C ，b e C =T e b
C )(
2．卡尔曼滤波
连续系统：
Z(t)=HX(t)+V(t) 离散系统：
w~N(0,Q) , v~N(0,R)。
惯性导航实验
惯性导航实验一、 实验目的1、了解惯性导航设备;2、掌握惯性导航设备的物理连接；3、掌握惯性导航信息的处理方法；4、掌握惯性导航方法并学会用编程实现惯性导航算法。
二、 实验器材YH-5000AHRS ；工业控制计算机；数据采集软件； 稳压电源；串口连接线； 三、 实验原理 （1） 姿态解算基于四元数法解算姿态矩阵。
p j p i p l Q +++=21 （1）⎥⎥⎥⎦⎤⎢⎢⎢⎣⎡=⎥⎥⎥⎦⎤⎢⎢⎢⎣⎡⎥⎥⎥⎦⎤⎢⎢⎢⎣⎡--++----+++---+=⎥⎥⎥⎦⎤⎢⎢⎢⎣⎡b b b p b b b b p p p z y x C z y x p p p l lp p p lp p p lp p p p p p l lp p p lp p p lp p p p p p l z y x 222123213223113223212223212313212322212)(2)(2)(2)(2)(2)(2 （2） b pbQw Q 21= （3） 上述微分方程表示成矩阵形式：⎥⎥⎥⎥⎦⎤⎢⎢⎢⎢⎣⎡⎥⎥⎥⎥⎥⎦⎤⎢⎢⎢⎢⎢⎣⎡------=⎥⎥⎥⎥⎥⎦⎤⎢⎢⎢⎢⎢⎣⎡321321000021p p p l w w w w w w w w w w w w p p p l b pbxb pbyb pbzb pbxb pbz bpbyb pby b pbz b pbx b pbz b pbyb pbx（4）初始四元数的确定计算如下：⎥⎥⎥⎥⎥⎥⎥⎥⎦⎤⎢⎢⎢⎢⎢⎢⎢⎢⎣⎡--++=⎥⎥⎥⎥⎦⎤⎢⎢⎢⎢⎣⎡2cos 2cos 2sin 2sin 2sin 2cos 2cos 2sin 2sin 2sin 2cos 2cos 2sin 2cos 2sin 2cos 2sin 2cos 2sin2sin2sin2cos2cos2cos )0(3)0(2)0()0(0000000000000000000001γθϕγθϕγθϕγθϕγθϕγθϕγθϕγθϕG G G G G G G G p p p l （5） 用四阶龙格库塔法解（4）的微分方程；⎥⎥⎥⎦⎤⎢⎢⎢⎣⎡=333231232221131211T T T T T T T T T C p b由p b C 中提取γϕλ，，G231sin T -=主λ22211tan T T G -=主ϕ )(tan 33131T T --=主γ 从而可得到：主λλ=⎪⎩⎪⎨⎧<>+>><+=0,020,002122212222T T T T T GG G G πϕϕπϕϕ主主主⎩⎨⎧<>=0,)(-0331333T T sign T πγγγ （2） 速率位置解算将加速度测量的沿坐标系轴向的比力bib a 转换成沿着导航坐标系轴向的比力p ib a ,则速度方程为:p p epp ep p ib p ep g V w a V +⨯+Ω-=)2( 展开得到：⎪⎩⎪⎨⎧-+Ω-+Ω+=+Ω+Ω-=+Ω-Ω+=gV w V w a V V w V a V V w V a V p epy p epx p x p epx p epy p y p ibz p epzp epz p epx p x p epx p z p iby p epy p epzp epy p y p epy p z p ibx p epx )2()2()2(2)2(2 由于Ω,pep w 都很小，故而速度方程简化为：⎪⎩⎪⎨⎧-===ga V a V a V p ibz pepz piby p epy p ibxp epx用一阶欧拉法解，则：⎪⎩⎪⎨⎧+-=++=++=+)(*)()()(*)()(*)(t V T g a T t V t V T a T t V t V T a T t V p epz p ibz p epzpepy p iby p epy pepx p ibx p epx 其中T 为采样时间。
惯性导航作业
惯性导航作业一、数据说明：1：惯导系统为指北方位的捷连系统。
初始经度为116.344695283度、纬度为39.975172度,高度h为30米。
初速度v0=[-9.993908270;0.000000000;0.348994967]。
2：jlfw中为600秒的数据，陀螺仪和加速度计采样周期分别为为1/100秒和1/100秒。
3：初始姿态角为[2 1 90]（俯仰，横滚，航向,单位为度）,jlfw.mat中保存的为比力信息f_INSc(单位m/s^2)、陀螺仪角速率信息wib_INSc(单位rad/s)，排列顺序为一~三行分别为X、Y、Z向信息.4: 航向角以逆时针为正。
5:地球椭球长半径re=6378245;地球自转角速度wie=7.292115147e-5;重力加速度g=g0*(1+gk1*c33^2)*(1-2*h/re)/sqrt(1-gk2*c33^2)；g0=9.7803267714;gk1=0.00193185138639;gk2=0.00669437999013;c33=sin(lat纬度);二、作业要求：1：可使用MATLAB语言编程，用MATLAB编程时可使用如下形式的语句读取数据：load D:\...文件路径...\jlfw,便可得到比力信息和陀螺仪角速率信息。
用角增量法。
2：(1) 以系统经度为横轴，纬度为纵轴（单位均要转换为：度）做出系统位置曲线图；(2) 做出系统东向速度和北向速度随时间变化曲线图（速度单位：m/s，时间单位：s）；(3) 分别做出系统姿态角随时间变化曲线图（俯仰，横滚，航向,单位转换为：度，时间单位：s）；以上结果均要附在作业报告中。
3：在作业报告中要写出“程序流程图、现阶段学习小结”，写明联系方式。
(注意程序流程图不是课本上的惯导解算流程，而是你程序分为哪几个模块、是怎样一步步执行的,什么位置循环等,让别人根据该流程图能够编出相应程序) (学习小结按条写，不用写套话) 4：作业以纸质报告形式提交，附源程序。
捷联惯导初始对准以及姿态解算
第三部分：基于“存储数据与迭代计算对准”罗经法对准
3.2 罗经法对准过程中的调整策略（以北向通道为例）
g
y
f
p N
1
VN
1
s
R
-
K1
K2 R
K3 s
Control algorithm
cE -
x
1
s
x
-
ie cos L
z
实线所示的北向通道：本质上是一个休拉回路，失准角作无阻尼振荡。
采取的策略：1）引入内反馈环节（虚线）实现衰减振荡；2）引入前馈环节（点画 线）缩短振荡周期；3）引入积分环节（双点画线）消除罗经项的影响。
3.5 SINS罗经法对准如何实现迭代计算？
fˆNn -
b ib
fb cU
Cˆbn
Cˆbn
b ib
Cnbine
Cnbc
cN
fˆ n Cˆbn f b
1
VN
s
1
cE
R
K1
K2 R
K3 s
Control algorithm
上述过程中，可以实现迭代计算。
Page 15
第三部分：基于“存储数据与迭代计算对准”罗经法对准
导航坐标系 n （b）SINS
GINS中的测量数据直接反映失准角的大小； SINS中的测量数据不直接反映失准角；只有投影数据能够反映失准角的大小；相同 的测量数据经过不同的姿态矩阵进行投影，可以获取不同的投影数据。 注：上述均不考虑仪表误差。
对于SINS而言，分析一种理想的情况：仪表无误差，载体无机动，此时在整个对准 过程中，仪表测量数据均相等。整个对准过程，其实只用了一组仪表参数。
3.6 SINS罗经法对准中存储数据如何使用？
捷联惯性导航系统的解算方法
捷联惯性导航系统的解算方法捷联惯性导航系统（Inertial Navigation System，简称INS）是一种利用陀螺仪和加速度计等惯性测量单元测量物体的加速度和角速度，然后通过对这些测量值的积分计算出物体的速度和位置的导航系统。
INS广泛应用于航空航天、无人驾驶车辆和船舶等领域，具有高精度和自主性等特点。
INS的解算方法一般分为初始对准、运动状态估计和航位推算三个主要过程。
初始对准是指在启动导航系统时，通过利用外部辅助传感器（如GPS）或静态校准等方法将惯性传感器的输出与真实姿态和位置进行初次校准。
在初始对准过程中，需要获取传感器的初始偏差和初始姿态，一般采用标定或矩阵运算等方法进行。
运动状态估计是指根据惯性传感器的测量值，使用滤波算法对物体的加速度和角速度进行实时估计。
常用的滤波算法包括卡尔曼滤波、扩展卡尔曼滤波和粒子滤波等。
其中，卡尔曼滤波是一种最优估计算法，通过对观测值和状态进行线性组合，得到对真实状态的最佳估计。
扩展卡尔曼滤波则是基于卡尔曼滤波的非线性扩展，可以应用于非线性INS系统。
粒子滤波是一种利用蒙特卡洛采样技术进行状态估计的方法，适用于非高斯分布的状态估计问题。
航位推算是指根据运动状态估计的结果，对物体的速度和位置进行推算。
INS最基本的航位推算方法是利用加速度值对速度进行积分，然后再对速度进行积分得到位置。
但是，在实际应用中，由于传感器本身存在噪声和漂移等误差，导致航位推算过程会出现积分漂移现象。
为了解决这个问题，通常采用辅助传感器（如GPS）和地图等数据对INS的输出进行校正和修正。
当前，还有一些先进的INS解算方法被提出，如基于深度学习的INS 解算方法。
这些方法利用神经网络等深度学习模型，结合原始传感器数据进行端到端的学习和预测，以实现更高精度的位置和姿态估计。
综上所述，捷联惯性导航系统的解算方法主要包括初始对准、运动状态估计和航位推算三个过程。
其中，运动状态估计过程利用滤波算法对传感器的测量值进行处理，得到物体的加速度和角速度的估计。
捷联惯导的解算程序
%====本程序为捷联惯导的解算程序(由惯性器件的输出解算出飞行器的位置、速度、姿态信息)======clear all;close all;clc;deg_rad=pi/180; %由度转化成弧度rad_deg=180/pi; %由弧度转化成度%-------------------------------从源文件中读入数据----------------------------------fid_read=fopen('IMUout.txt','r'); %path1_Den.dat 是由轨迹发生器产生的数据[AllDataNumofAllData]=fscanf(fid_read,'%g %g %g %g %g %g %g %g %g %g %g %g %g %g %g %g',[17 inf]);AllData=AllData';NumofEachData=round(NumofAllData/17);Time=AllData(:,1);longitude=AllData(:,2); %经度单位：弧度latitude=AllData(:,3); %纬度单位：弧度High=AllData(:,4); %高度单位：米Ve=-AllData(:,6); % 东向、北向、天向速度单位：米/妙Vn=AllData(:,5);Vu=AllData(:,7);fb_x=AllData(:,9); %比力（fx，fy，fz）fb_y=AllData(:,8); %指向右机翼方向为x正方向，指向机头方向为y正向，z轴与x轴和y轴构成右手坐标系单位：米/秒2fb_z=-AllData(:,10); %右前上pitch=AllData(:,11); %俯仰角（向上为正）单位:弧度head=-AllData(:,13); %偏航角（偏西为正）roll=AllData(:,12); %滚转角（向右为正）omigax=AllData(:,15); %陀螺输出（单位：弧度/秒，坐标轴的定义与比力的相同）omigay=AllData(:,14);omigaz=-AllData(:,16);%-------------------------------程序初始化--------------------------------------latitude0=latitude(1);longitude0=longitude(1); %初始位置High0=High(1);Ve0=Ve(1);Vn0=Vn(1); %初始速度Vu0=Vu(1);pitch0=pitch(1);head0=head(1); %初始姿态roll0=roll(1);TimeEach=0.005; %周期和仿真总时间TimeAll=(NumofEachData-1)*TimeEach;Omega_ie=0.7292115147E-4;%0.00007272205216643040; %地球自转角速度单位：弧度每妙g0=9.78;%------------------------------导航解算开始--------------------------------------%假设没有初始对准误差pitch_err0=pitch0+0*deg_rad;head_err0=head0+0*deg_rad;roll_err0=roll0+0*deg_rad;%初始捷联矩阵的计算《捷联惯导系统》P63 旋转顺序 head - pitch - roll %导航坐标系n为东北天方向载体坐标系b为右前上偏航角北偏西为正Tbn(1,1)=cos(roll_err0)*cos(head_err0)-sin(roll_err0)*sin(pitch_err0) *sin(head_err0);Tbn(1,2)=cos(roll_err0)*sin(head_err0)+sin(roll_err0)*sin(pitch_err0) *cos(head_err0);Tbn(1,3)=-sin(roll_err0)*cos(pitch_err0);Tbn(2,1)=-cos(pitch_err0)*sin(head_err0);Tbn(2,2)=cos(pitch_err0)*cos(head_err0);Tbn(2,3)=sin(pitch_err0);Tbn(3,1)=sin(roll_err0)*cos(head_err0)+cos(roll_err0)*sin(pitch_err0) *sin(head_err0);Tbn(3,2)=sin(roll_err0)*sin(head_err0)-cos(roll_err0)*sin(pitch_err0) *cos(head_err0);Tbn(3,3)=cos(roll_err0)*cos(pitch_err0);Tnb=Tbn';%位置矩阵的初始化《捷联惯导系统》P46 其中游动方位角 a＝0 假使初始经纬度确知Cne(1,1) = - sin(longitude0);Cne(1,2) = cos(longitude0);Cne(1,3) = 0;Cne(2,1) = - sin(latitude0) * cos(longitude0);Cne(2,2) = - sin(latitude0) * sin(longitude0);Cne(2,3) = cos(latitude0);Cne(3,1) = cos(latitude0) * cos(longitude0);Cne(3,2) = cos(latitude0) * sin(longitude0);Cne(3,3) = sin(latitude0);Cen=Cne';%初始四元数的确定《捷联惯导系统》 P151-152 方法本身保证了q1^2+q2^2+q3^2+q4^2=1q(2,1) = sqrt(abs(1.0 + Tnb(1,1) - Tnb(2,2) - Tnb(3,3))) / 2.0; q(3,1) = sqrt(abs(1.0 - Tnb(1,1) + Tnb(2,2) - Tnb(3,3))) / 2.0; q(4,1) = sqrt(abs(1.0 - Tnb(1,1) - Tnb(2,2) + Tnb(3,3))) / 2.0;q(1,1) = sqrt(abs(1.0 - q(2,1) ^2 - q(3,1) ^2 - q(4,1) ^2));% 判断q(1,1)的符号flag_q11=cos(head_err0/2.0)*cos(pitch_err0/2.0)*cos(roll_err0/2.0)-sin(head_err0/2.0)*sin(pitch_err0/2.0)*sin(roll_err0/2.0);if (flag_q11 >0) %此时q(1,1)取正if (Tnb(3,2) < Tnb(2,3))q(2,1) = - q(2,1);endif (Tnb(1,3) < Tnb(3,1))q(3,1) = - q(3,1);endif (Tnb(2,1) < Tnb(1,2))q(4,1) = - q(4,1);endelse %此时q(1,1)取负或0q(1,1) = - q(1,1);if (Tnb(3,2) > Tnb(2,3))q(2,1) = - q(2,1);endif (Tnb(1,3) > Tnb(3,1))q(3,1) = - q(3,1);endif (Tnb(2,1) > Tnb(1,2))q(4,1) = - q(4,1);endend%-------------------------迭代推算用到的参数的初始化------------------------Wiee_e = 0;Wiee_n = 0;Wiee_u = Omega_ie;Wiee = [Wiee_e Wiee_n Wiee_u]'; %地球速率在地球系中的投影东－北－天Lat_err(1)=latitude0;Lon_err(1)=longitude0;High_err(1)=High0;Ve_err(1)=Ve0;Vn_err(1)=Vn0;Vu_err(1)=Vu0;pitch_err(1)=pitch_err0;head_err(1)=head_err0;roll_err(1)=roll_err0;Re=6378137.0;%6378245.0; %地球长轴《惯性导航系统》 P28e=0.0033528106647474807198455286185206; %地球扁率精确值ee=0.00669437999014131699614;%----------------------------迭代推算开始-----------------------------------for i=1:NumofEachData%----------------------------惯性仪表数据的获得------------------------Wibb(1,1)=omigax(i); %指向右机翼方向为x正方向，指向机头方向为y正向，z轴与x轴和y轴构成右手坐标系Wibb(2,1)=omigay(i); %单位：弧度/妙Wibb(3,1)=omigaz(i); %右前上fb(1,1)=fb_x(i); %指向右机翼方向为x正方向，指向机头方向为y正向，z轴与x轴和y轴构成右手坐标系fb(2,1)=fb_y(i); %单位：米/秒2fb(3,1)=fb_z(i); %右前上%--------计算在姿态矩阵和位置矩阵更新时用到的参数------------------ RM=Re*(1.0-2.0*e+3.0*e*Cne(3,3)^2)+High_err(i); %《捷联惯导系统》P233 P235RN=Re*(1.0+e*Cne(3,3)^2)+High_err(i);% RN=Re*(1-ee)/(sqrt(1-ee*sin(Lat_err(i))))^3+High_err(i);% RM=Re/sqrt(1-ee*sin(Lat_err(i)))+High_err(i);%实验当地重力加速度计算《捷联惯导系统》P150 《惯性导航系统》 P35g=g0*((1.0+0.0052884*Cne(3,3)^2)-0.0000059*(1-(1-2*Cne(3,3)^2)^2))*(1 .0-2.0*High_err(i)/Re);tmp_slat=sin(Lat_err(i))*sin(Lat_err(i));Wien = Cne * Wiee; %地球速率在导航系中的投影Wenn(1,1) = -Vn_err(i)/RM;Wenn(2,1) = Ve_err(i)/RN; % <<惯性导航系统>> P45 考虑了地球转动的影响.Wenn(3,1) = Ve_err(i)*tan(Lat_err(i))/RN; %计算Wenn(不太精确)，更新速度和位置矩阵时用Winn=Wien+Wenn;Winb=Tbn*Winn;Wnbb=Wibb-Winb; %姿态速率在姿态更新时用到fn=Tnb*fb; % x-y-z 东－北－天% 速度的更新《捷联惯导系统》 P30 33 东－北－天difVe_err=fn(1,1)+(2*Wien(3,1)+Wenn(3,1))*Vn_err(i)-(2*Wien(2,1)+Wenn (2,1))*Vu_err(i);difVn_err=fn(2,1)-(2*Wien(3,1)+Wenn(3,1))*Ve_err(i)+(2*Wien(1,1)+Wenn (1,1))*Vu_err(i);difVu_err=fn(3,1)+(2*Wien(2,1)+Wenn(2,1))*Ve_err(i)-(2*Wien(1,1)+Wenn (1,1))*Vn_err(i)-g;Ve_err(i+1)=Ve_err(i)+difVe_err*TimeEach;Vn_err(i+1)=Vn_err(i)+difVn_err*TimeEach;Vu_err(i+1)=Vu_err(i)+difVu_err*TimeEach;High_err(i+1)=High_err(i)+Vu_err(i)*TimeEach;% 位置矩阵的实时更新《惯性导航系统》 P190Cne(1,1)=Cne(1,1)+TimeEach*(Wenn(3,1)*Cne(2,1)-Wenn(2,1)*Cne(3,1)); Cne(1,2)=Cne(1,2)+TimeEach*(Wenn(3,1)*Cne(2,2)-Wenn(2,1)*Cne(3,2)); Cne(1,3)=Cne(1,3)+TimeEach*(Wenn(3,1)*Cne(2,3)-Wenn(2,1)*Cne(3,3)); Cne(2,1)=Cne(2,1)+TimeEach*(-Wenn(3,1)*Cne(1,1)+Wenn(1,1)*Cne(3,1)); Cne(2,2)=Cne(2,2)+TimeEach*(-Wenn(3,1)*Cne(1,2)+Wenn(1,1)*Cne(3,2)); Cne(2,3)=Cne(2,3)+TimeEach*(-Wenn(3,1)*Cne(1,3)+Wenn(1,1)*Cne(3,3)); Cne(3,1)=Cne(3,1)+TimeEach*(Wenn(2,1)*Cne(1,1)-Wenn(1,1)*Cne(2,1)); Cne(3,2)=Cne(3,2)+TimeEach*(Wenn(2,1)*Cne(1,2)-Wenn(1,1)*Cne(2,2)); Cne(3,3)=Cne(3,3)+TimeEach*(Wenn(2,1)*Cne(1,3)-Wenn(1,1)*Cne(2,3));% Mat_Wenn(1,1)=0;% Mat_Wenn(1,2)=Wenn(3,1);% Mat_Wenn(1,3)=-Wenn(2,1); %Wenn的反对阵矩阵取负% Mat_Wenn(2,1)=-Wenn(3,1); %这里位置矩阵的及时修正为:dCne/dt=Mat_Wenn*Cne% Mat_Wenn(2,2)=0;% Mat_Wenn(2,3)=Wenn(1,1);% Mat_Wenn(3,1)=Wenn(2,1);% Mat_Wenn(3,2)=-Wenn(1,1);% Mat_Wenn(3,3)=0;%% Mat_Wenn=Mat_Wenn*Cne*TimeEach;% Cne=Cne+Mat_Wenn;Cen=Cne';% 计算经纬度Lat_err(i+1)=asin(Cne(3,3));Lon_err(i+1)=atan(Cne(3,2)/Cne(3,1)); %这是经度的主值if (Cne(3,1) < 0)if (Lon_err(i+1) > 0)Lon_err(i+1) = Lon_err(i+1) - pi;elseLon_err(i+1) = Lon_err(i+1) + pi;endend% 四元数的及时修正《惯性导航系统》 P194% Mat_Wnbb=[ 0, -Wnbb(1,1), -Wnbb(2,1), -Wnbb(3,1); % Wnbb(1,1), 0, Wnbb(3,1), -Wnbb(2,1); % Wnbb(2,1), -Wnbb(3,1), 0, Wnbb(1,1); % Wnbb(3,1), Wnbb(2,1), -Wnbb(1,1), 0];% q=q+Mat_Wnbb*q*TimeEach/2.0;q(1,1)=q(1,1)+TimeEach*(-Wnbb(1,1)*q(2,1)-Wnbb(2,1)*q(3,1)-Wnbb(3,1)* q(4,1))/2.0;q(2,1)=q(2,1)+TimeEach*(Wnbb(1,1)*q(1,1)+Wnbb(3,1)*q(3,1)-Wnbb(2,1)*q (4,1))/2.0;q(3,1)=q(3,1)+TimeEach*(Wnbb(2,1)*q(1,1)-Wnbb(3,1)*q(2,1)+Wnbb(1,1)*q (4,1))/2.0;q(4,1)=q(4,1)+TimeEach*(Wnbb(3,1)*q(1,1)+Wnbb(2,1)*q(2,1)-Wnbb(1,1)*q (3,1))/2.0;% 四元数归一化处理q_norm=sqrt(sum(q.*q));q=q/q_norm;% 计算姿态矩阵 TnbTnb(1,1) = q(1,1) ^2 + q(2,1) ^2 - q(3,1)^2 - q(4,1)^2;Tnb(1,2) = 2.0 * (q(2,1) * q(3,1) - q(1,1) * q(4,1));Tnb(1,3) = 2.0 * (q(2,1) * q(4,1) + q(1,1) * q(3,1));Tnb(2,1) = 2.0 * (q(2,1) * q(3,1) + q(1,1) * q(4,1));Tnb(2,2) = q(1,1)^2 - q(2,1)^2 + q(3,1)^2 - q(4,1)^2;Tnb(2,3) = 2.0 * (q(3,1) * q(4,1) - q(1,1) * q(2,1));Tnb(3,1) = 2.0 * (q(2,1) * q(4,1) - q(1,1) * q(3,1));Tnb(3,2) = 2.0 * (q(3,1) * q(4,1) + q(1,1) * q(2,1));Tnb(3,3) = q(1,1)^2 - q(2,1)^2 - q(3,1)^2 + q(4,1)^2;Tbn=Tnb';flag_pitch=asin(Tnb(3,2));flag_roll=atan(-Tnb(3,1)/Tnb(3,3));flag_head=atan(-Tnb(1,2)/Tnb(2,2));if(Tnb(3,3)<0)if(flag_roll<0)flag_roll=flag_roll+pi;endif(flag_roll>0)flag_roll=flag_roll-pi;endend% 偏航角范围 -180度——180度北偏西为正if(Tnb(2,2)<0)if(flag_head<0)flag_head=flag_head+pi;endif(flag_head>0)flag_head=flag_head-pi;endend% 姿态角更新pitch_err(i+1)=flag_pitch;head_err(i+1)=flag_head;roll_err(i+1)=flag_roll;% 解算完毕由对准结果、陀螺、加表的输出解算出载体的位置、速度、姿态%----------------------计算解算误差------------------ddLat(i)=(Lat_err(i)-latitude(i))*rad_deg; %纬度误差单位：度ddLog(i)=(Lon_err(i)-longitude(i))*rad_deg; %经度误差单位：度ddHigh(i)=High_err(i)-High(i); %高度误差单位：米ddVe(i)=Ve_err(i)-Ve(i);ddVn(i)=Vn_err(i)-Vn(i); % 速度误差单位：米/妙2ddVu(i)=Vu_err(i)-Vu(i);ddpitch(i)=(pitch_err(i)-pitch(i))*rad_deg*3600; %姿态误差单位：度ddhead(i)=(head_err(i)-head(i))*rad_deg*3600;ddroll(i)=(roll_err(i)-roll(i))*rad_deg*3600;endfclose(fid_read);%---------------------------绘图开始--------------------------------- figure(1)plot(Time,ddLog)ylabel('经度误差(度)'),xlabel('时间(秒)');figure(2)plot(Time,ddLat)ylabel('纬度误差(度)'),xlabel('时间(秒)');figure(3)plot(Time,ddHigh);ylabel('高度误差(米)'),xlabel('时间(秒)');figure(4)plot(Time,ddhead)ylabel('偏航角误差(角妙)'),xlabel('时间(秒)'); figure(5)plot(Time,ddpitch)ylabel('俯仰角误差(角妙)'),xlabel('时间(秒'); figure(6)plot(Time,ddroll);ylabel('滚转角误差(角妙)'),xlabel('时间(秒)'); figure(7)plot(Time,ddVe);ylabel('东向速度误差(米/秒)'),xlabel('时间(秒)'); figure(8)plot(Time,ddVn)ylabel('北向速度误差(米/秒)'),xlabel('时间(秒)'); figure(9)plot(Time,ddVu)ylabel('天向速度误差(米/秒)'),xlabel('时间(秒)');%------------------------------绘图结束-------------------------------。
惯导位置解算
惯导位置解算
惯导位置解算（Inertial Navigation System, INS）是一种基
于惯性力和角速度的位置和速度测量技术，可以用于航空、航天和军
事等领域。
它的基本原理是通过测量惯性传感器感受到的力和加速度，利用运动学和动力学方程计算出自身的位置、速度和方向。
惯导位置解算的步骤：
1. 惯性传感器测量
INS利用的惯性传感器包括加速度计和陀螺仪。
加速度计可以测量自身在各个方向的加速度，依据牛顿第二定律计算出自身的速度和位置。
陀螺仪则可测量自身的角速度，并根据初始角度测量计算出自身的方向。
2. 运动学方程
利用加速度计测量的速度和陀螺仪测量的角速度，可以得到自身的位移，即INS的运动学方程。
运动学方程描述了动态系统中的物体位置
和速度随时间的变化关系。
3. 动力学方程
除了运动学方程外，动力学方程还需要考虑自身的质量和惯性力。
利
用牛顿第二定律可以得到INS的动力学方程。
此方程描述了动态系统
中物体的加速度随时间的变化。
4. INS组合算法
INS组合算法可以用于修正INS误差，并结合其他传感器（如GPS）的
测量结果，以获得更加准确的位置和速度。
INS组合算法采用卡尔曼滤波器、粒子滤波器和扩展卡尔曼滤波器等算法，以提高定位的精度和
鲁棒性。
总的来说，惯导位置解算是一种高精度的位置和速度测量技术，
具有良好的抗干扰能力和适应性。
在卫星通信、智能交通、无人机等
领域中有着广泛应用。
imu惯导matlab解算
imu惯导matlab解算
在MATLAB中解算惯性导航（IMU）可以使用以下步骤：
1. 准备数据：你需要收集IMU传感器的数据，包括加速度计和陀螺仪的测量值。
这些数据通常以时间序列的形式提供。
2. 读取数据：使用MATLAB的文件读取函数（例如`readmatrix`）将数据读取到MATLAB的工作空间中。
3. 数据预处理：IMU数据通常需要进行预处理，例如去除噪声、校准传感器和对数据进行滤波。
可以使用MATLAB的信号处理工具箱进行这些预处理步骤。
4. 设计滤波器：在IMU数据预处理之后，可能需要设计一个滤波器来进一步减少噪声和提高数据质量。
MATLAB的信号处理工具箱提供了多种滤波器设计方法。
5. 解算姿态：使用IMU的加速度计和陀螺仪测量值，可以通过解算算法来估计姿态。
常见的解算算法包括加速度计积分法、四元数解算、互补滤波器等。
可以使用MATLAB中的数学函数和算法来实现这些解算算法。
6. 可视化结果：可以使用MATLAB的绘图函数（例如`plot`）将解算结果可视化，以便更好地理解IMU的姿态估计。
请注意，解算IMU的精确性和准确性可能需要对算法进行调优，并进行对比和评估。
IMU还可能受到环境因素和传感器误差的影响，因此在解算IMU数据时需要考虑到这些因素。
车载捷联惯导系统定位测姿算法研究
第１５卷第ｌ期２００７年２月中国惯性技术学报ＪｏｕｍａｌｏｆＣｈｉｎｅｓｅＩｎｅｒｔｉａｌＴｃｃｈｎｏｌｏｇｙＶｂｌ．１５Ｎｏ．１Ｆｅｂ．２００７文章编号：１００５－６７３４（２００７）０１一００２４－０４车载捷联惯导系统定位测姿算法研究陈允芳１，叶泽田２，钟若飞３（１．山东科技大学地球信息科学与工程学院，青岛２６６５１０；２．中国测绘科学研究院，北京１０００３９；３．首都师范大学，北京１０００３７）摘要：ＧＰｓ／ＩＮｓ组合精确测定平台的位置和姿态是移动测图系统中的重要模块。
对陀螺仪和加速度计所测角速度和比力进行两次积分得载体姿态、速度和位置即ｓＩＮｓ力学机械编排。
目前该过程大多在地理坐标系进行。
这里详细推导了地球坐标系中完整的解算过程，以四元数姿态矩阵更新及重力计算为核心，由ＩＭｕ原始观测值解算出了载体位置、速度和姿态等参数，可快速高效与ＧＰｓ输出的位置速度信息进行组合滤波处理，可据此编程进行工程应用数据处理。
关键词：捷联惯导系统；姿态矩阵；坐标转换；力学编排；四元数中图分类号：ｕ６６６．１文献标识码：ＡＰｏｓｉｔｉｏｎｉｎｇａｎｄｏｒｉｅｎｔａｔｉｏｎｃｏｍｐｕｔａｔｉｏｎｏｎＶｅｈｉｃｌｅ－ｂｏｒｎｅＳＩＮＳａｎｄｄｉｓｃｕｓｓｏｆｃａｌｃｕｌａｔｉｏｎｅｒｒｏｒｃＨＥＮＹｕｎ．‰９１，ＹＥｚｅ－ｔｉａｎ２，ｚＨＯＮＧＲｕｏ．ｆｅｉ３（１．Ｇｅｏ·ｉｎｆｏ衄ａｔｉｏｎＳｃｉｅｎｃｅ＆ＥｎｇｉｎｅｅｒｉｎｇＣｏｌｌｅｇｅ，ＳｈａｎｄｏｎｇＵｎｉｖｅｒＳｉ哆ｏｆＳｃｉｅｎｃｅａＩｌｄＴｂｃｈｎｏｌｏｇｙ，Ｑｉｎｇｄａｏ２６６５１０，Ｃｈｉｎａ；２．ＳｕｒｖｅｙｉｎｇａＴｌｄＭａｐｐｉｎｇＳｃｉｅｎｃｅＲｅｓｅａｒｃｈＩｎＳｔｉｔｕｔｅｏｆＣｈｉｎａ，Ｂｅｉｊｉｎｇ１０００３９，Ｃｈｉｎａ；３．Ｃ印ｉｔａｌＮｏｍｌａｌＵｎｉｖｅｒＳｉ劬Ｂｅｉｊｉｎｇ１０００３７，Ｃｈｉｎａ）Ａｂｓｔｒａｃｔ：ＧＰＳａｎｄＩＮＳｉｎｔｅｇｒａｔｅｄｔｏａｃｃｕｒａｔｅｌｙｄｅｔｅｍｉｎｉｎｇｐｏｓｉｔｉｏｎａＩｌｄａｔｔｉｔｕｄｅｏｆｎａｔＩ‘ｏｏｆｉｓＶｉｔａｌｍｏｄｕｌｅｉｎｍｏｂｉｌｅｍａｐｐｉｎｇＳｙｓｔｅｍ．Ｓｐｅｃｉｎｃｆｏｒｃｃ行ｏｍｓｐｅｅｄｏｍｅｔｅｒ蚰ｄ舭ｇｌｅｒａｔｅ矗ｏｍ留ｒｏａｒｅｉｎｔｅ铲ａｔｅｄｔｗｉｃｅｒｅｓｐｅｃｔｉＶｅｌｙｔｏａｃｈｉｅｖｅａｎ沁ｄｅ，ｖｅｌｏｃ时ａＩｌｄｐｏｓｉｔｉｏｎｎ锄ｅｌｙＳＩＮＳｍｅｃｈａＩｌｉｚａｔｉｏｎ．Ｃｕｒｒｅｎｔｌｙｔｈｉｓｔｏｏｋｐｌａｃｅｄｉｎｇｅｏｇｒ印ｈｉｃｃｏｏｒｄｉｎａｔｅ，ｗｈｉＩｅｈｅｒｅｄｅｍｏｎｓｔｒａｔｅｄｉｎｄｅｔａｉｌｍｅｗｈｏｌｅｍｅｃｈａＪｌｉｚａｔｉｏｎｉｎｅａｎｌｌ－ｃｅｎｔｃｌｒｃｄｅａｒｔｈ－ｆｉｘｅｄｃｏｏｒｄｉｎａｔｅ，ｍｏｓｔｌｙｑｕａｔｅｍｉｏｎａ钍ｉｔｕｄｅｍａｔｒｉｘｕｐｄａｔｉｎｇ锄ｄｇｒａｖｉｔ）ｒｃａＩｃｕｌａｔｉｏｎ．Ｕｌｔｉｍａｔｅｌｙｖｅｈｉｃｌｅｎａｖｉｇａｔｉｏｎｐａｒ锄ｅｔｅｒｓｓｕｃｈａＳａｔｔｉｔｕｄｅ，ｖｅＩｏｃｉｔｙ锄ｄｐｏｓｉｔｉｏｎｗｅｒｃｇａｈｅｄ丘ｏｍＩＭＵｏｒｉｇｉｎ“０ｂｓｅｒｖａｔｉｏｎｓ．Ｍａｔｈｅｍａｔｉｃｓｐｌａｔｆｏ眦ｉｓｆｏｍｌｃｄｉｎＳｒＮＳｔｏｃａｒｒｙｏｕｔｓｕⅣｅｙｉｎｇａＪｌｄｃａｌｃｕｌａｔｉｎｇｐｒｅｃｉｓｅｌｙｔｈｅｎａｖｉｇａｔｉｏｎｍｏＶｅｍｅｎｔｐａｒ锄ｃｔｅｒＳ．Ｔｈｅｒｅｓｕｌｔｓａｒｃｐｒｏｎｅｔｏｉｎｔｅ黟ａｔｅｗｉｔｌｌｓｉｍｉｌａｒｐａｍｍｅｔｅｒｓ疔ｏｍＧＰＳｔｏｆｉｌｔｅｒｐｒｏｃｅｓｓｉｎｇ．Ｐｒｏ可锄ｍｉｎｇｈｅｒｃｂｙｃ锄ｐｍｃｅｓｓｄａｔａｉｎｅｎｇｉｎｅｅｒｉｎｇａｐｐｌｉｃａｔｉｏｎＫｅｙｗｏｒｄｓ：ＳＩＮＳ；ａｔｔｉｔｕｄｅｍａｔｒｉｘ；ｃｏｏｒｄｉｎａｔｅｔｒａｎｓｆｏｍａｔｉｏｎ；ｍｅｃｈａｎｉｚａｔｉｏｎ；ｑｕａｔｅｍｉｏｎ随着惯性技术与卫星导航定位技术的发展，由ＧＰＳ／ＩＮｓ不同程度组合而成的定位定姿传感器已成为移动测图系统中确定载体轨迹和平台姿态的重要工具，其中ＧＰｓ多用于定位而ＩＮＳ则用于测姿。
捷联惯导结算原理
0 cos sin ， Rz sin 0 cos
sin cos 0
0 0 1
cos cos sin sin sin cos cos sin sin cos sin cos T11 T12 T13 Ry Rx Rz cos sin cos cos sin T21 T22 T23 sin cos cos sin sin sin sin cos sin cos cos cos T T T 31 32 33 b 由姿态矩阵 C n 反解飞行器姿态欧拉角：
（5） 速度的计算
t t t t t 0 2iez etz ety 2iey Vxt Vx 0 t t b t t t t 0 2iex etx Vyt 0 Vy Cb f 2iez etz t Vz g Vzt 2 t t 2 t t 0 iey ety iex etx
o o sin 1 T23 , 90 , 90
tg 1
T13 180o , 180o , T33
tg 1
T21 o o , 180 , 180 T 22
图 6 东向北向速度变化曲线
阶段总结：1.学习了平台式和捷联式惯导的惯导解算方法并进行了仿真计算。 2.平台式惯导物理平台时刻跟踪当地水平东北天地理系， 加速计的比 力信息直接投影在导航系中，可直接进行导航速度和位置解算。载体的姿态可直 接从平台框架直接得出；而捷联式惯导用数学平台取代实体的物理平台，通过求
惯导位置解算
惯导位置解算
惯导位置解算是一种基于惯性导航技术的位置解算方法，它可以通过测量惯性传感器的输出来计算出目标的位置和姿态信息。
惯导位置解算技术在航空、航天、军事等领域得到了广泛的应用，具有高精度、高可靠性、高稳定性等优点。
惯导位置解算技术的基本原理是利用惯性传感器测量目标的加速度和角速度，然后通过积分计算出目标的速度和位移。
惯性传感器包括加速度计和陀螺仪，加速度计用于测量目标的加速度，陀螺仪用于测量目标的角速度。
通过对加速度和角速度的测量和积分，可以得到目标的速度和位移信息。
惯导位置解算技术的优点在于它可以独立于外部环境进行位置解算，不受GPS信号等外部因素的影响。
因此，在没有GPS信号或GPS 信号不稳定的情况下，惯导位置解算技术可以提供可靠的位置解算结果。
此外，惯导位置解算技术还可以提供高精度的姿态信息，可以用于目标的导航、控制和稳定等方面。
惯导位置解算技术的应用范围非常广泛，包括航空、航天、军事、海洋、地震等领域。
在航空领域，惯导位置解算技术可以用于飞机、导弹、卫星等的导航和控制；在航天领域，惯导位置解算技术可以用于卫星的姿态控制和轨道控制；在军事领域，惯导位置解算技术可以用于导弹、战斗机、坦克等的导航和控制；在海洋领域，惯导位置解算技术可以用于船舶、潜艇等的导航和控制；在地震领域，
惯导位置解算技术可以用于地震监测和预警。
惯导位置解算技术是一种非常重要的位置解算方法，具有高精度、高可靠性、高稳定性等优点，可以在各种复杂环境下提供可靠的位置解算结果。
随着科技的不断发展，惯导位置解算技术将会得到更广泛的应用和发展。