捷联惯导Matlab程序求解飞行器的姿态
matlab姿态解算
matlab姿态解算Matlab姿态解算是一种通过数学模型和算法，将传感器采集到的数据转化为姿态信息的技术。
姿态解算在航空航天、机器人、虚拟现实等领域具有重要的应用价值。
本文将介绍Matlab姿态解算的原理、方法和应用。
我们来了解姿态解算的概念。
姿态是指物体在空间中的方向和位置。
在三维空间中，姿态通常由欧拉角或四元数表示。
姿态解算的目标是根据传感器采集到的数据，推导出物体的姿态信息。
在Matlab中，姿态解算可以通过多种方法实现。
其中一种常用的方法是基于加速度计和陀螺仪的姿态解算。
加速度计可以测量物体在三个方向上的加速度，陀螺仪可以测量物体绕三个方向上的角速度。
通过对这些数据的处理和分析，可以推导出物体的姿态信息。
姿态解算的核心是将传感器数据转化为姿态信息的数学模型。
在Matlab中，可以使用旋转矩阵或四元数来表示姿态信息。
旋转矩阵是一个3x3的矩阵，可以表示物体在三维空间中的旋转变换。
四元数是一种复数扩展，可以用来表示旋转变换。
在Matlab中，可以使用一些现有的工具箱来实现姿态解算。
例如，可以使用Robotics System Toolbox中的函数来进行姿态解算。
这些函数提供了一些常用的姿态解算算法，例如扩展卡尔曼滤波（EKF）和互补滤波器。
除了基于加速度计和陀螺仪的姿态解算，还有其他一些方法可以用于姿态解算。
例如，可以使用磁力计来测量地球的磁场，从而推导出物体的方向信息。
还可以使用视觉传感器来获取物体在相机坐标系中的姿态信息。
姿态解算在许多领域具有广泛的应用。
在航空航天领域，姿态解算可以用于导航、飞行控制和目标跟踪等任务。
在机器人领域，姿态解算可以用于机器人的定位和路径规划。
在虚拟现实领域，姿态解算可以用于头部追踪和手部追踪等应用。
总结起来，Matlab姿态解算是一种通过数学模型和算法，将传感器采集到的数据转化为姿态信息的技术。
姿态解算在航空航天、机器人、虚拟现实等领域具有重要的应用价值。
@捷联惯导系统姿态解算模块的实现---具体实现过程
收稿日期 :2008208207 ; 修订日期 :2008209205. 基金项目 : 吉林省杰出青年基金资助项目 ( No . 20060115) ; 中国科学院 “三期创新” 平台资助项目
© 1994-2009 China Academic Journal Electronic Publishing House. All rights reserved.
摘要 : 提出了一种实现捷联惯性导航系统姿态解算模块的方法 。基于 DSP 的硬件平台和四元数的数学平台 ,设计了捷 联惯性导航系统的姿态解算模块 。介绍了捷联惯性导航系统的工作原理和姿态解算的基本算法 , 并给出了四元数法的 四阶龙格2库塔数值解法 。设计了姿态解算模块的硬件电路和软件实现程序 。实验测试结果表明 ,在增量角 < 5° 的情况 下 ,用四阶龙格2库塔法进行姿态解算 , 误差 < 0. 005 3 % ; 应用 TMS320C6713B 进行硬件电路设计 , 每次解算时间 <
和调试 。综上所述 ,DSP 系统具有速度快 、 精度 高、 体积小 、 成本低 、 可靠性高以及抗干扰能力强 等优点 ,可以满足捷联式惯性导航系统对硬件平 台的要求 [ 122 ] 。
© 1994-2009 China Academic Journal Electronic Publishing House. All rights reserved.
,
( 5)
2
K23 T
2
如下 : ψ θ γ ψ θ γ 0 0 0 0 0 0 cos cos cos + sin sin sin 2 2 2 2 2 2 λ (0) ψ θ γ ψ θ γ 0 0 0 0 0 0 cos cos sin - sin sin cos p1 (0) 2 2 2 2 2 2 = , ψ θ γ ψ θ γ p2 (0) 0 0 0 0 0 0 cos sin cos + sin cos sin 2 2 2 2 2 2 p3 (0) ψ θ γ ψ θ γ 0 0 0 0 0 0 sin cos cos - cos sin sin 2 2 2 2 2 2
捷联惯性导航系统的姿态算法研究的开题报告
捷联惯性导航系统的姿态算法研究的开题报告
标题：捷联惯性导航系统的姿态算法研究
一、研究背景
随着现代科技的不断进步，无人飞行器（UAV）的应用越来越广泛，而惯性导航系统作为实现无人飞行器自主飞行的核心设备之一，在飞行控制系统中发挥着重要作用。
其中，姿态算法是惯性导航系统的关键技术之一，能够实现无人飞行器稳定飞行和精确控制。
二、研究目的
本文旨在研究捷联惯性导航系统的姿态算法，探究其改进和优化方法，提高其稳定性和精度，为无人飞行器的自主飞行提供更加可靠的支持。
三、研究内容
（一）姿态解算
姿态解算是捷联惯性导航系统中姿态算法的核心问题。
本文将研究基于四元数的姿态解算方法，并探讨姿态解算的实时性和精度。
（二）滤波算法
针对捷联惯性导航系统中存在的传感器噪声和测量误差等问题，本文将研究常用的滤波算法，如卡尔曼滤波和扩展卡尔曼滤波等，并探讨其在姿态解算中的应用。
（三）姿态控制算法
在实际应用中，无人飞行器需要通过姿态控制实现目标飞行姿态的调整和保持。
本文将研究基于四元数的姿态控制算法，并分析其控制精度和实时性等关键技术。
四、研究方法
本研究将采用理论分析、仿真计算和实验验证相结合的方法，从理论上探究捷联惯性导航系统的姿态算法优化方法，并通过仿真计算和实验验证对算法的效果进行评估。
五、预期成果
本文将研究捷联惯性导航系统的姿态算法，包括姿态解算、滤波算法和姿态控制算法等关键技术。
预期成果为优化和改进现有的算法，提高捷联惯性导航系统的精度和稳定性，为无人飞行器的自主飞行提供可靠的支持。
Matlab在飞行器设计与控制中的应用指南
Matlab在飞行器设计与控制中的应用指南飞行器设计与控制是航空领域中至关重要的技术领域之一。
实现一个高性能、稳定可靠的飞行器需要精确的设计和控制算法。
而Matlab作为一种强大的数值计算工具和开发环境，在飞行器设计与控制中发挥着至关重要的作用。
本文将重点介绍Matlab在飞行器设计与控制中的应用指南。
1. 飞行器建模与仿真飞行器的设计与控制首先需要建立准确的数学模型。
Matlab提供了丰富的工具箱和函数，可以方便地进行飞行器的建模和仿真。
首先，可以利用Matlab的Simulink工具进行连续系统和离散系统的建模。
通过建立准确的飞行动力学方程和传感器模型，并结合各种环境因素，如空气动力学和风扰动，可以得到真实可靠的仿真结果。
此外，Matlab还可以使用SimMechanics工具箱进行多体动力学建模，以更精确地描述飞行器的运动。
2. 飞行器姿态控制飞行器的姿态控制是保持飞行器稳定飞行的核心问题。
Matlab为飞行器姿态控制提供了丰富的控制设计和分析工具。
例如，可以使用Matlab内置的Control System Toolbox来设计和优化飞行器的控制器，并通过频域分析和根轨迹等工具评估系统的稳定性和性能。
此外，Matlab还提供了强大的优化工具，如优化和鲁棒控制工具箱，可以帮助用户通过自动化方法获得最优的控制器参数。
3. 导航与定位在飞行器设计与控制过程中，导航与定位是不可或缺的。
Matlab提供了一套完整的导航和定位算法工具箱，可以方便地进行导航滤波、轨迹规划、姿态解算等操作。
例如，可以使用自适应卡尔曼滤波算法对飞行器的姿态和位置进行准确估计。
此外，Matlab还提供了GPS和惯性导航系统的仿真工具，可以模拟不同环境下的导航和定位性能。
4. 通信与数据处理在现代飞行器中，通信与数据处理起着关键的作用。
Matlab提供了一系列用于通信系统设计和数据处理的工具箱，如通信工具箱、图像处理工具箱等。
Matlab技术在飞行器导航中的应用案例分享
Matlab技术在飞行器导航中的应用案例分享随着航空事业的迅猛发展，飞行器的导航系统越来越重要。
无人机、航天器和民航飞机等飞行器的导航精度要求越来越高，对导航算法的需求也变得更为复杂。
在这方面，Matlab技术发挥了重要的作用。
本文将通过几个实际案例，分享Matlab技术在飞行器导航中的应用。
案例一：无人机航迹规划无人机的航迹规划是一项关键任务，它确定了无人机的轨迹和航线。
Matlab具备强大的数学计算和图形绘制功能，可以帮助无人机航迹规划专家进行快速且精确的计算。
通过Matlab，专家可以编写航迹规划算法，并将其可视化展示。
这使得无人机航迹规划人员可以更好地理解无人机的轨迹，并做出优化点评。
案例二：卫星导航系统设计卫星导航系统是现代飞行器导航的核心，例如全球定位系统（GPS）。
在卫星导航系统设计中，Matlab被广泛用于信号处理、接收机设计和导航算法验证。
用Matlab编写的仿真程序可以模拟各种导航场景，并评估系统性能。
此外，Matlab还支持与硬件设备的接口，可以实现实时的导航算法验证。
案例三：传感器融合与状态估计在飞行器导航中，传感器融合和状态估计是至关重要的。
在传感器融合过程中，来自不同传感器的数据被整合，以获得更准确的飞行器状态信息。
而状态估计则是根据传感器融合结果推测飞行器的位置和姿态。
Matlab拥有丰富的滤波器设计和优化工具，可以帮助工程师实现高效且稳定的传感器融合与状态估计算法。
案例四：飞行器导航性能评估对飞行器导航性能的评估是及时优化和改进导航系统的有效手段。
Matlab提供了多种性能评估工具，如误差分析、性能指标计算和数据可视化等。
通过这些工具，工程师可以分析导航系统的弱点，并针对性地进行改进。
同时，Matlab还支持批量处理数据，使得对于大量导航数据的性能评估更为高效。
总结Matlab技术在飞行器导航中发挥了重要的作用。
它不仅帮助实现了高精度的航迹规划和卫星导航系统设计，还支持传感器融合与状态估计的优化以及导航性能的评估。
捷联系统航姿算法分析
们可以利用前一时刻的角速度采样值对旋
转矢量双子样法中的角增量进行修正 ,由此 4 数字仿真
提出双子样改进算法 。
我们用 MATLAB 编写了仿真程序 ,采用
修正后 ,双子样法的角增量由以下公式 角速度输出的陀螺 ,在圆锥运动下进行了数
提取
字仿真 。不同算法的时间复杂度取捷联矩
Δθ1 = [ 8ω(t - T2/ 2) - ω(t) + 5ω(t + T2/ 阵一次递推所需运算次数 ,如下表所示
<20 8
+ζ·3<8404 ,d2≈0. 5 -
<20 48
Δ; (10) Δθ3 = [ω(t + 2T3/ 3) +ω(t + T3) ]·T3/ 6 +Δ;
式中 ζ, = 0. 8 ,目的是为了减小漂移误差 。 若认为在式 (5) 中 ,一个计算周期内ω
= a + b·t ,并作一定的近似 ,可推导出等效 旋转矢量双子样计算公式为
自动化技术与应用 Vol. 19 ,No. 1 2000 年第 1 期 7
q (t + T1) = q (t) + (k1 + k2 + k3 + k4) / 6
若认为在一个计算周期内ω= a + b·t +
(7) c·t2 ,可导出等效旋转矢量三子样计算公式
式中 ,T1 为计算周期 ; k1 = 0. 5·T1[ωb (t) ]·q (t) ; k2 = 0. 5·T1 [ωb (t + T1/ 2) ] ·( q (t) + k1/
对于双子样法 Δθ1 = [ω(t) +ω(t + T2/ 2) ]·T2/ 4 ;
捷联惯导Matlab程序
捷联惯导程序，依据加表和陀螺仪的输出数据来求解飞行器的姿态clc;clear;format long; %设置数据精度为15位小数Data=importdata('temp.txt'); % 导入实验所采集的数据，以矩阵形式赋给Data变量，temp.txt 必须与该M文件在同一个文件夹中Px=Data(:,3); % Px,Py,Pz为陀螺仪的输出值Py=Data(:,4);Pz=Data(:,5);Nx=Data(:,6); % Nx,Ny，Nz为加速度计的输出值Ny=Data(:,7);Nz=Data(:,8);% 陀螺仪模型参数标定如下：Sx = -4.085903e-006 ; Sy = -4.085647e-006 ; Sz = -4.085170e-006 ;Mxy = 5.059527e-003 ; Mxz = -1.031103e-003 ; Myx = -3.355451e-003 ;Myz = 3.508468e-003 ; Mzx = -1.266671e-003 ; Mzy = -2.318244e-004 ;Dx = -2.009710e-006 ; Dy = 8.156346e-007 ; Dz = -5.749059e-007 ;GyroCali_A = [ 1 -Mxy -Mxz ; -Myx 1 -Myz ; -Mzx -Mzy 1 ];% 加速度计模型参数标定如下：Kx = 9.272930e-004 ; Ky = 9.065544e-004 ; Kz = 9.443748e-004 ;Ixy = 6.533872e-003 ; Ixz = 9.565992e-004 ; Iyx = -6.319376e-003 ;Iyz = -6.902339e-004 ; Izx = -1.144549e-003 ; Izy = -3.857963e-004 ;Bx = -3.400847e-002 ; By = -8.916341e-003 ; Bz = -9.947414e-003 ;AccCali_A = [1 -Ixy -Ixz ; -Iyx 1 -Iyz ; -Izx -Izy 1 ];Delta_t = 0.05; %采样时间为0.05秒Delta_Theta_x = 0;Delta_Theta_y = 0;Delta_Theta_z = 0; %定义陀螺仪输出的角度增量Delta_Vx = 0;Delta_Vy = 0;Delta_Vz = 0; %定义加速度计输出的速度增量L = zeros(1,12001);L(1)= 45.7328*pi/180 ; %纬度用L表示，纬度的初始值划为弧度形式，因为后面计算位置矩阵更新L(2)= 45.7328*pi/180 ; %时需要用到前两次的L值来计算当前L值，所以在此定义2个初始L值Lamda = 126.6287*pi/180 ; %经度用Lamda表示，经度的初始值划为弧度形式h = 136 ; %高度用h表示V = [ 0 ; 0 ; 0 ]; %导航坐标系中的东北天初始速度都为0Vx = 0; %方便后面的速度计算与速度更新Vy = 0;Vz = 0;Theta = 0;Gama = 0;Fai = 0; %初始姿态角（俯仰角/倾斜角/航向角）都为0,此处均为弧度Re = 6378254 ; Rp = 6356803 ;%定义地球的半长轴与半短轴e = (Re - Rp)/Re ; %定义旋转椭球扁率（椭球度）Wie = 15.04107/180*pi ; %定义地球自转角速度，地球坐标系相对于惯性坐标系的角速度Theta_Matrix = zeros(1,12000); %定义姿态角矩阵，供画图用Gama_Matrix = zeros(1,12000);Fai_Matrix = zeros(1,12000);L_Matrix = zeros(1,12001); %定义经纬度矩阵，供画图用，L的特殊性决定了其数据个数为12001L_Matrix(1) = 45.7328;Lamda_Matrix = zeros(1,12000);Ve_Matrix = zeros(1,12000); %定义速度矩阵，供画图用Vn_Matrix = zeros(1,12000);Vu_Matrix = zeros(1,12000);%以下计算捷联矩阵的初始值，捷联矩阵的初始值仅仅由Theta，Gama，Fai的初始值决定T = [ cos(Gama)*cos(Fai)-sin(Gama)*sin(Theta)*sin(Fai) -cos(Theta)*sin(Fai) sin(Gama)*cos(Fai)+cos(Gama)*sin(Theta)*sin(Fai) ;cos(Gama)*sin(Fai)+sin(Gama)*sin(Theta)*cos(Fai) cos(Theta)*cos(Fai) sin(Gama)*sin(Fai)-cos(Gama)*sin(Theta)*cos(Fai) ;-sin(Gama)*cos(Theta) sin(Theta) cos(Gama)*cos(Theta) ];%由捷联矩阵的初始值计算初始四元数值，为捷联矩阵的实时更新做准备if(T(3,2)-T(2,3)>0)Q1 = 0.5*sqrt(1+T(1,1)-T(2,2)-T(3,3));else if (T(3,2)-T(2,3)==0)Q1 = 0;else Q1 = -0.5*sqrt(1+T(1,1)-T(2,2)-T(3,3));end %求解Q1endif(T(1,3)-T(3,1)>0)Q2 = 0.5*sqrt(1-T(1,1)+T(2,2)-T(3,3));else if (T(1,3)-T(3,1)==0)Q2 = 0;else Q2 = -0.5*sqrt(1-T(1,1)+T(2,2)-T(3,3));end %求解Q2endif(T(2,1)-T(1,2)>0)Q3 = 0.5*sqrt(1-T(1,1)-T(2,2)+T(3,3));else if (T(2,1)-T(1,2)==0)Q3 = 0;else Q3 = -0.5*sqrt(1-T(1,1)-T(2,2)+T(3,3));end %求解Q3endQ0 = 0.5*sqrt(1-Q1*Q1-Q2*Q2-Q3*Q3); %求解Q0Q = [Q0 ; Q1 ; Q2 ; Q3]; %四元数初始值Q = Q / sqrt(Q0*Q0+Q1*Q1+Q2*Q2+Q3*Q3); %四元数的初始归一化，为得到最小漂移误差%以下求位置矩阵的初始值，通过位置矩阵更新后，反过来算运载体所在的经纬度%位置矩阵仅仅与经纬度有关系，Ce2n表示把地球坐标系转换为导航坐标系的转换矩阵Ce2n = [ -sin(Lamda) cos(Lamda) 0 ;-sin( L(1) )*cos(Lamda) -sin(L(1))*sin(Lamda) cos( L(1) );cos( L(1) )*cos(Lamda) cos( L(1) )*sin(Lamda) sin( L(1) ) ];%大循环，共执行12000次，实时更新捷联矩阵，速度矩阵，位置矩阵，保存作图所需数据for k = 1:12000;GyroCali_B = [Sx*Px(k)-Dx*Delta_t ; Sy*Py(k)-Dy*Delta_t ; Sz*Pz(k)-Dz*Delta_t ];Delta_Theta = GyroCali_A * GyroCali_B ; %计算陀螺仪输出的角度增量Delta_Theta_x = Delta_Theta(1);Delta_Theta_y = Delta_Theta(2);Delta_Theta_z = Delta_Theta(3);Delta_Theta_Module = sqrt( Delta_Theta_x * Delta_Theta_x + Delta_Theta_y * Delta_Theta_y + Delta_Theta_z * Delta_Theta_z );AccCali_B = [Kx*Nx(k)-Bx*Delta_t ; Ky*Ny(k)-By*Delta_t ; Kz*Nz(k)-Bz*Delta_t ];Delta_V = AccCali_A * AccCali_B ; %计算加速度计输出的速度增量Delta_Vx = Delta_V(1);Delta_Vy = Delta_V(2);Delta_Vz = Delta_V(3);Delta_V_Module = sqrt( Delta_Vx * Delta_Vx + Delta_Vy * Delta_Vy + Delta_Vz * Delta_Vz );%使用毕卡法求解四元数更新矩阵，即捷联矩阵Bika = zeros(4);Bika(1,1) = cos(0.5 * Delta_Theta_Module);Bika(1,2) = -Delta_Theta_x / Delta_Theta_Module * sin(0.5 * Delta_Theta_Module);Bika(1,3) = -Delta_Theta_y / Delta_Theta_Module * sin(0.5 * Delta_Theta_Module);Bika(1,4) = -Delta_Theta_z / Delta_Theta_Module * sin(0.5 * Delta_Theta_Module);Bika(2,1) = -Bika(1,2);Bika(2,2) = Bika(1,1);Bika(2,3) = -Bika(1,4);Bika(2,4) = Bika(1,3);Bika(3,1) = -Bika(1,3);Bika(3,2) = -Bika(2,3);Bika(3,3) = Bika(1,1);Bika(3,4) = -Bika(1,2);Bika(4,1) = -Bika(1,4);Bika(4,2) = -Bika(2,4);Bika(4,3) = -Bika(3,4);Bika(4,4) = Bika(1,1);Q = Bika * Q; % 每循环一次，更新一次四元素Q值，为求捷联矩阵Q = Q / sqrt(Q0*Q0+Q1*Q1+Q2*Q2+Q3*Q3); %四元数的归一化，为得到最小漂移误差Q0 = Q(1);Q1 = Q(2);Q2 = Q(3);Q3 = Q(4);%捷联矩阵的四元数表达式T = [ Q0*Q0+Q1*Q1-Q2*Q2-Q3*Q3 2*(Q1*Q2-Q0*Q3) 2*(Q1*Q3+Q0*Q2)2*(Q1*Q2+Q0*Q3) Q0*Q0-Q1*Q1+Q2*Q2-Q3*Q3 2*(Q2*Q3-Q0*Q1)2*(Q1*Q3-Q0*Q2) 2*(Q2*Q3+Q0*Q1) Q0*Q0-Q1*Q1-Q2*Q2+Q3*Q3 ];%*********************************************************************%********************求三个姿态角Theta，Gama和Fai ******************** %*********************************************************************Theta_Main = asin( T(3,2) );Gama_Main = atan( -T(3,1) / T(3,3));Fai_Main = atan( -T(1,2) / T(2,2));Theta = Theta_Main;if (T(3,3)>0)Gama = Gama_Main ;else if (T(3,3)<0 && Gama_Main > 0)Gama = Gama_Main + pi;else Gama = Gama_Main - pi; %此处用else实为不妥，不过为了程序的完善性，只能这样了endendif ( T(2,2)<0 )Fai = Fai_Main + pi ;else if (T(2,2)==0)Fai = pi/2;else if ( Fai_Main>0)Fai = Fai_Main;else Fai = Fai_Main + 2*pi ;endendend%以下存储姿态角到三个矩阵里面，为画图做准备Theta_Matrix(k) = Theta*180/pi; %作图用矩阵,以角度表示Gama_Matrix(k) = Gama*180/pi; %作图用矩阵,以角度表示if (Fai<2*pi)Fai_Matrix(k) = Fai*180/pi;else Fai_Matrix(k) = Fai*180/pi-360; %作图用矩阵,以角度表示end%到此为止，姿态角的求解完毕，以下先求速度%*********************************************************************%********************求飞行器相对于东北天的速度************************* %*********************************************************************Rm = Re*( 1-2*e+3*e*sin( L(k+1) )^2 );Rn = Re*( 1+e*sin(L(k+1))^2 );LL = 3/2*L(k+1) - 1/2*L(k) ;F = [ 0 -1/(Rm + h) 0 ; 1/(Rn + h) 0 0 ; tan( LL )/(Rn + h) 0 0];g = 9.7803+0.051799*sin(L(k+1))^2-0.94114e-006*h;G = [0;0;-g];Wen2n = F*V;Wie2n = [0 ; Wie*cos(L(k+1)); Wie*sin(L(k+1))];W = 2*Wie2n + Wen2n;W_X = [ 0 -W(3) W(2) ; W(3) 0 -W(1) ; -W(2) W(1) 0 ]; %此式中W_X为2*Wie+Wen2n 的反对称矩阵V = V + T*( Delta_V+ [ 0 -Delta_Theta_z Delta_Theta_y ; Delta_Theta_z 0 -Delta_Theta_x ; -Delta_Theta_y Delta_Theta_x 0 ]*Delta_V ) + Delta_t*(G-W_X*V);Vx = V(1);Vy = V(2);Vz = V(3);V e_Matrix(k) = V(1);Vn_Matrix(k) = V(2);Vu_Matrix(k) = V(3);%*********************************************************************%********************求飞行器所在的经纬度****************************** %*********************************************************************Epsilon = F*V*Delta_t;Ce2n =( eye(3) - [ 0 -Epsilon(3) Epsilon(2) ; Epsilon(3) 0 -Epsilon(1) ; -Epsilon(2) Epsilon(1) 0 ] )*Ce2n ; %位置矩阵实时更新%下面通过位置矩阵来实时更新经纬度L(k+2) = asin( Ce2n(3,3)); %由于L本来就是以矩阵形式定义的，下面定义一个把L用角度表示的矩阵L_Matrix(k+1) = L(k+2)*180/pi;Lamda_Main = atan( Ce2n(3,2)/Ce2n(3,1) ); %计算出来的L和Lamda都是弧度制的if (Ce2n(3,1)>0)Lamda = Lamda_Main;else if (Lamda_Main<0)Lamda = Lamda_Main + pi;else Lamda = Lamda_Main - pi;endendLamda_Matrix(k) = Lamda*180/pi; %作图用矩阵，以角度表示end%**************************************************************************%********************以下是画图程序*****************************************%**************************************************************************k=1:1:12000; %绘制三轴姿态变化图线-绿色figure(1);plot(k/20,Theta_Matrix(k),'g');xlabel('Time(second)');ylabel('Angle(degree)');title('Theta(俯仰角)');grid on;figure(2);plot(k/20,Gama_Matrix(k),'g');xlabel('Time(second)');ylabel('Angle(degree)');title('Gama（滚转角）');grid on;figure(3);plot(k/20,Fai_Matrix(k),'m');xlabel('Time(second)');ylabel('Angle(degree)');title('Fai（偏航角）');grid on;%绘制东北天个方向的速度变化曲线-红色figure(4);plot(k/20,Ve_Matrix(k),'r');xlabel('Time(second)');ylabel('Velocity(m/s)');title('Ve（东向速度）');grid on;figure(5);plot(k/20,Vn_Matrix(k),'r');xlabel('Time(second)');ylabel('Velocity(m/s)');title('Vn（北向速度）');grid on;figure(6);plot(k/20,Vu_Matrix(k),'r');xlabel('Time(second)');ylabel('Velocity(m/s)');title('Vu（天向速度）');grid on;%绘制飞行器所在经纬度曲线-蓝色figure(7);plot(k/20,L_Matrix(k),'b');xlabel('Time(second)');ylabel('Degree');title('Latitude L(纬度)');grid on;figure(8);plot(k/20,Lamda_Matrix(k),'b');xlabel('Time(second)');ylabel('Degree');title('Longitude Lamda(经度)');grid on;。
捷联惯导的解算程序
%====本程序为捷联惯导的解算程序(由惯性器件的输出解算出飞行器的位置、速度、姿态信息)======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('时间(秒)');%------------------------------绘图结束-------------------------------。
捷联式惯性导航积分算法设计（连载）上篇：姿态计算
捷联式惯性导航积分算法设计（连载）上篇：姿态计算
卢军政
【期刊名称】《江南航天科技》
【年(卷),期】2001(000)002
【摘要】本论文分上下两篇，给用于现代捷联惯导系统的主要软件算法设计提供
一个严密的综合方法：将角速率积分成姿态角，将加速度变换或积分成速度以及将速度积分成位置。
该算法是用两速修正法构成的，而两速修正法是具有一定创新程度的新颖算法，是为姿态修正而开发出来的，在姿态修正中，以中速运用精密解析方程去正积分参数（姿态，速度或位置），其输入是由在参数修正（姿态锥化修正，速度遥橹修正以及高分辨率位置螺旋修正）时间间隔内计算运动角速度和加速度的高速算法提供的，该设计方法考虑了通过捷联系统惯性传感器对角速度或比力加速度所进行的测量以及用于姿态其准和矢量速度积分的导航系旋转问题。
本论文上篇定义了捷联惯导积分函数的总体设计要求，并开发出了用于姿态修正算法的方向余弦法和四元数法，下篇着重讨论速度和位置积分算法的设计。
尽管上下两篇讨论中常常涉及到基本的惯性导航概念。
然而，本论文是为那些已对基础惯导概念很熟悉的实际工作者而写的。
【总页数】20页(P14-33)
【作者】卢军政
【作者单位】江南航天集团302所
【正文语种】中文
【中图分类】V249.322
【相关文献】
1.惯性导航系统航向姿态计算算法研究
2.捷联式惯性导航计算机控制系统
3.捷联惯性导航积分算法设计--第一部分：姿态算法
4.捷联惯性导航积分算法设计
5.捷联惯性导航积分算法设计
因版权原因，仅展示原文概要，查看原文内容请购买。
四轴飞行器1.4姿态解算和Matlab实时姿态显示
四轴飞⾏器1.4姿态解算和Matlab实时姿态显⽰原创⽂章，欢迎转载，转载请注明出处MPU6050数据读取出来后，经过⼀个星期的努⼒，姿态解算和在matlab上的实时显⽰姿态终于完成了。
1：完成matlab的串⼝，并且实时通过波形显⽰数据2：添加RTT查看CPU使⽤率的扩展功能，MPU6050读取数据的优化3：四元素表⽰的坐标变化，四元素与欧拉⾓的关系和Madgwick的IMUupdate算法4：飞控数据采集线程和数据处理线程的安排，类似于⽣产者与消费者的关系。
先放个效果视频。
1：matlab串⼝初始化还是⽐较简单的，⽹上的资料也很多，这⾥就直接贴初始化代码了。
1 % --- Executes on button press in pb_OpenSerialPort.2 function pb_OpenSerialPort_Callback(hObject, eventdata, handles)3 % hObject handle to pb_OpenSerialPort (see GCBO)4 % eventdata reserved - to be defined in a future version of MATLAB5 % handles structure with handles and user data (see GUIDATA)6 %7global o_SerialPort;8 %______________________________________________9 %GUI全局变量101112 %---------------------串⼝初始化-----------------------13 %%%COM端⼝初始化14 int_Index_COM=get(handles.pop_SerialPort,'Value');15 string_COM=get(handles.pop_SerialPort,'String');16 string_Select_COM=string_COM{int_Index_COM};17 o_SerialPort=serial(string_Select_COM);18 %%%Baud初始化19 int_Index_Baud=get(handles.pop_BaudRate,'Value');20 string_Baud=get(handles.pop_BaudRate,'String');21 string_Select_Baud=string_Baud{int_Index_Baud};22 double_Baud=str2double(string_Select_Baud);23set(o_SerialPort,'BaudRate',double_Baud);24 %%%设置数据长度25 int_Index_DataBit=get(handles.pop_DataBit,'Value');26 string_DataBit=get(handles.pop_DataBit,'String');27 string_Select_DataBit=string_DataBit(int_Index_DataBit);28 double_DataBit=str2double(string_Select_DataBit);29set(o_SerialPort,'DataBits',double_DataBit);30 %%%设置停⽌位长度31 int_Index_StopBits=get(handles.pop_StopBits,'Value');32 string_StopBits=get(handles.pop_StopBits,'String');33 string_Select_StopBits=string_StopBits(int_Index_StopBits);34 double_StopBits=str2double(string_Select_StopBits);35set(o_SerialPort,'StopBits',double_StopBits);36 %%%设置输⼊缓冲区⼤⼩为1M37set(o_SerialPort,'InputBufferSize',1024000);38 %%%串⼝事件回调设置3940set(o_SerialPort,'BytesAvailableFcnMode','terminator');41set(o_SerialPort,'terminator','!'); %!标识结束符结束，⽅便处理和读取数据4243 o_SerialPort.BytesAvailableFcn={@EveBytesAvailableFcn,handles};44 % ----------------------打开串⼝-----------------------45 fopen(o_SerialPort);matlab串⼝我们采⽤回调函数，类似于中断⽅式哈，但是mtalb的串⼝⼗分的不好⽤哈，没有多线程，⽽我们在中断⾥⾯需要进⾏波形显⽰，四元素旋转等各种数据操作，是需要花费点时间的，这就导致我们的数据平率不能很⾼。
基于MATLAB的捷联惯组测量数据分析平台
基于MATLAB的捷联惯组测量数据分析平台作者：陈春歌宋玉珍来源：《数字技术与应用》2020年第09期摘要：针对某型飞行器的运动轨迹参数的计算问题，建立了基于MATLAB的捷联惯组测量数据分析平台。
该平台采用MATLAB GUI图形化编程语言编制，功能设计兼具模块化和结构化，通过设定初始参数生成飞行器运动轨迹。
利用仿真试验数据结果表明，该平台实现了捷联惯组数据在发射坐标下的解算，可用于某飞行器试验数据事后处理和运动轨迹复现工作。
关键词：飞行器;捷联惯组;运动轨迹;MATLAB GUI中图分类号：TP273 文献标识码：A 文章编号：1007-9416（2020）09-0161-040 引言某飞行器惯性测量装置用于测量飞行器相对惯性空间转动的角速度和视加速度在载体坐标系三个轴上的投影[1]。
然而在实际应用中，由于测量所得角速率和加速度为载体坐标下数据，在事后数据处理中需将其转换到发射坐标系下进行分析，以复现出发射坐标下的弹道数据，并需要将数据进行存储再利用。
MATLAB GUI是基于MATLAB的图形化编程语言，基于MATLAB GUI的编程既能有效利用MATLAB的计算功能，又能通过GUI直观显示计算结果及曲线等，能更直观地分析飞行器飞行弹道特征及捷联惯组性能。
本文捷联惯组测量数据分析平台各部分功能相互独立，初始参数、原始测量数据读取部分根据飞行器的运动特征和初始环境，设定初始参数选取原则，并在弹道复现计算中进行了初始误差补偿;导航数据解算部分是分析平台的核心部分，对飞行器的姿态矩阵更新选取了两种算法：欧拉角法和四元数法，实现了两种算法下姿态解算的精度比较;该平台另一特色即是结合飞行器飞行弹道特征点，选取主要特征点进行解算分析。
通过设置不同初始参数，复现不同初始条件下各时刻载体的运动信息、姿态信息及弹道特征点信息，解算结果明确，便于工程技术人员进行事后数据分析。
1 捷联惯组测量数据分析建模1.1 选取坐标系捷联惯性测量装置固连于飞行器载体上，测量到的是载体坐标系轴向上的分量，飞行器运动轨迹的观测点在发射坐标系，初始运动参数的设置建立在艇体坐标系[1]。
用四元数法的捷联惯性导航姿态解算程序
;no dirg ;)')s/m( /yticoleV'(lebaly ;)')sm01( / emiT'(lebalx ;)'noitaluclaC yticoleV'(eltit ;)'Z :etanidrooC noitagivaN','Y :etanidrooC noitagivaN','X :etanidrooC noitagivaN'(dnegel )yticolev,mt(tolp erugif ;no dirg ;)')eerged( /legnA'(lebaly ;)')sm01( / emiT'(lebalx ;)'noitaluclaC erutseG'(eltit ;)'legnA waY','legnA lloR','legnA hctiP'(dnegel ;)'.-b',):,3(ELGNA,mt,'.g',):,2(ELGNA,mt,'-r',):,1(ELGNA,mt(tolp ;ip/081*ELGNA=ELGNA erugif dne
值比度速加/压电的向轴个三计度速加% ?=za_orez% ?=ya_orez% ?=xa_orez% 压电点零的向轴个三计度速加%
*********************************************************% % 值度速加的应相为换转据数将% 3*4201/cca_atad=cca_atad ;]z_cca;y_cca;x_cca[=cca_atad 压电拟模为换转字数度速加% %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% ;)'txt.zetarelecca'(daol=z_cca ;)'txt.yetarelecca'(daol= y_cca ;)'txt.xetarelecca'(daol= x_cca ;4201/3=etar_cca% **************据数的计度速加入读****************% ;)'txt.zoryg'(daol=z_oryg ;)'txt.yoryg'(daol=y_oryg ;)'txt.xoryg'(daol=x_oryg 据数的仪螺陀入读**********% 据数入读****************************% ;']g-,0,0[=G 2^s/m8.9 位单%;24108.9=g 量矢度速加的生产力重% ;lla raelc ;lla esolc
用四元数法的捷联惯性导航姿态解算程序
用四元数法的捷联惯性导航姿态解算程序close all;clear all;%重力产生的加速度矢量g=9.80142;%单位9.8m/s^2G=[0,0,-g]';%****************************读入数据%**********读入陀螺仪的数据gyro_x=load('gyrox.txt');gyro_y=load('gyroy.txt');gyro_z=load('gyroz.txt');%****************读入加速度计的数据**************%acc_rate=3/1024;acc_x =load('acceleratex.txt');acc_y =load('acceleratey.txt');acc_z=load('acceleratez.txt'); %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% %加速度数字转换为模拟电压data_acc=[acc_x;acc_y;acc_z];data_acc=data_acc/1024*3%将数据转换为相应的加速度值%%*********************************************************%加速度计三个轴向的零点电压%zero_ax=?%zero_ay=?%zero_az=?%加速度计三个轴向的电压/加速度比值%rate_ax=? %单位是m/s^2/V%rate_ay=?%rate_az=?%acc_x=acc_x*acc_rate;%acc_y=acc_y*acc_rate;%acc_z=acc_z*acc_rate;aver_acc_x=mean(acc_x)aver_acc_y=mean(acc_y)aver_acc_z=mean(acc_z)%采样时间dtime=0.01;tm=0:dtime:0.01* (size(gyro_x,2)-1);%个数numn_point=size(gyro_x,2);%图1figureplot(tm,data_acc(1,:),'-',tm,data_acc(2,:),'.',tm,data_acc(3,:),'-.'); title('加速度计的采样曲线');legend('x_ACC','Y_ACC','Z_ACC');xlabel('Time / (10ms)');ylabel('Accelerate/ (m/s'')');grid on;%plot(tm,acc_x,'-',tm,acc_y,'.',tm,acc_z,'-.');%title('加速度的计的采样曲线'):%对采样曲线进行低通滤波a=[1,2,4,2,1];%gyro_x=filter(a/sum(a),1,gyro_x);%gyro_y=filter(a/sum(a),1,gyro_y);%gyro_z=filter(a/sum(a),1,gyro_z);%比例变换gyro_x=gyro_x/1024*3/0.6;gyro_y=gyro_y/1024*3/0.6;gyro_z=gyro_z/1024*3/0.6;%零点电压--陀螺仪，取前80个数的平均电压zero_gx=sum(gyro_x(1:80))/80zero_gy=sum(gyro_y(1:80))/80zero_gz=sum(gyro_z(1:80))/80%减去零点gyro_x=(gyro_x-zero_gx)/0.0125/180*pi; gyro_y=(gyro_y-zero_gy)/0.0125/180*pi; gyro_z=(gyro_z-zero_gz)/0.0125/180*pi;%gyro_x=(gyro_x-2.5)/0.0125/180*pi;%gyro_y=(gyro_y-2.5)/0.0125/180*pi;%gyro_z=(gyro_z-2.5)/0.0125/180*pi;%测试数据accelerate=zeros(3,n_point);accelerate(1,1:100)=10;accelerate(1,101:200)=-10;accelerate(1,201:300)=0;%陀螺仪数据gyro_x=zeros(1,n_point);gyro_y=zeros(1,n_point);gyro_z=zeros(1,n_point);gyro_z(1:100)=pi/3;gyro_z(101:200)=-pi/3;%重力轴始终有加速度accelerate(3,:)=accelerate(3,:)+9.8;figureplot(tm,accelerate(1,:),'-',tm,accelerate(2,:),'.',tm,accelerate(3,:),'-.');title('加速度计的采样曲线');legend('x_ACC','Y_ACC','Z_ACC');xlabel('Time / (10ms)');ylabel('Accelerate/ (m/s'')');grid on;%画出陀螺仪的采样曲线figureplot(tm,gyro_x,'r-',tm,gyro_y,'g.',tm,gyro_z,'b-.');title('陀螺仪的采样曲线');legend('x_Gyro','Y_Gyro','Z_Gyro');xlabel('Time / (10ms)');ylabel('Angel_rate/ (degree/s)');grid on;%size(gyro_x)%size(gyro_y)%size(gyro_z)data_gyro=[gyro_x;gyro_y;gyro_z];%转移矩阵--即方向余弦矩阵T=eye(3); %T是3*3的单位矩阵,初始转移矩阵%四元数矩阵，存储每步更新之后的四元数，方便以后绘图Q=zeros(4,n_point);%四元数的初始值确定，假定一开始导航坐标系与载体坐标系是重合的，因此方向余弦矩阵，是单位矩阵，利用它们之间的关系确定四元数的初始值。
捷联惯导系统姿态算法比较
捷联惯导系统姿态算法比较
捷联惯导系统姿态算法比较
姿态算法是捷联惯导系统算法中的一个重要组成部分,解算姿态阵相当于建立起数学平台,其精度对捷联惯导系统的精度影响很大.该文就实际应用,对欧拉角法、方向余弦法、四元数算法、罗德利格参数法、优化旋转矢量算法及一种改进的递推旋转矢量算法做了分析,并在典型圆锥运动输入下,对后五种算法进行了仿真,为姿态算法的研究提供了参考.
作者：孙丽秦永元 SUN Li QIN Yong-yuan 作者单位：西北工业大学自动化学院,西安,710072 刊名：中国惯性技术学报ISTIC PKU 英文刊名：JOURNAL OF CHINESE INERTIAL TECHNOLOGY 年，卷(期)： 2006 14(3) 分类号： U666.1 关键词：捷联姿态算法精度圆锥运动比较。
捷联系统的四元数法姿态算法
捷联系统的四元数法姿态算法算法输入：物体的初始姿态，3轴陀螺仪不同时刻的Yaw，Pitch，Roll的角速度；算法输出：物体的当前姿态。
具体算法：1. 初始姿态的四元数(w,x,y,z)=(1,0,0,0) 命名为A2. 读取3轴陀螺仪当前时刻的Yaw,Pitch,Roll角速度，乘以上次计算以来的间隔时间，得到上一时刻以来(Yaw,Pitch,Roll)的变化量，命名为欧拉角b3. b是Tait-Bryan angle定义的欧拉角，将其转为四元数B4. A=A×B，做四元数乘法，即可得到当前姿态对应的新的四元数A5.重复2～4部，即可连续更新姿态6.将四元数A重新转换为Tait-Bryan angle形式的欧拉角a，就可以以直观的形式查看当前姿态核心算法1，欧拉角转四元数void Quaternion::FromEulerAngle(const EulerAngle &ea){float fCosHRoll = cos(ea.fRoll * .5f);float fSinHRoll = sin(ea.fRoll * .5f);float fCosHPitch = cos(ea.fPitch * .5f);float fSinHPitch = sin(ea.fPitch * .5f);float fCosHYaw = cos(ea.fYaw * .5f);float fSinHYaw = sin(ea.fYaw * .5f);w = fCosHRoll * fCosHPitch * fCosHYaw + fSinHRoll * fSinHPitch * fSinHYaw;x = fCosHRoll * fSinHPitch * fCosHYaw + fSinHRoll * fCosHPitch * fSinHYaw;y = fCosHRoll * fCosHPitch * fSinHYaw - fSinHRoll * fSinHPitch * fCosHYaw;z = fSinHRoll * fCosHPitch * fCosHYaw - fCosHRoll * fSinHPitch * fSinHYaw;}核心算法2，四元数转欧拉角EulerAngle Quaternion::ToEulerAngle() const{EulerAngle ea;ea.fRoll = atan2(2 * (w * z + x * y) , 1 - 2 * (z * z + x * x));ea.fPitch = asin(CLAMP(2 * (w * x - y * z) , -1.0f , 1.0f));ea.fYaw = atan2(2 * (w * y + z * x) , 1 - 2 * (x * x + y * y));return ea;}核心算法3，四元数乘法Quaternion Quaternion::Multiply(const Quaternion &b){Quaternion c;c.w=w*b.w -x*b.x -y*b.y -z*b.z;c.x=w*b.x +x*b.w +y*b.z -z*b.y;c.y=w*b.y -x*b.z +y*b.w +z*b.x;c.z=w*b.z +x*b.y -y*b.x +z*b.w;c.Normalize();return c;}次要的规范化算法：void Quaternion::Normalize(){float s=getS();w/=s;x/=s;y/=s;z/=s;}float Quaternion::getS(){return sqrt(w*w+x*x+y*y+z*z);}我的loop函数，算法的集成部分：Quaternion nowQ;void loop() {int intx, inty,intz;float pitch,roll,yaw;gyro.ReadGyroOutCalibrated_Radian(&pitch, &roll, &yaw); EulerAngle dt;dt.fRoll=roll;dt.fPitch=pitch;dt.fYaw=-yaw;Quaternion dQ;dQ.FromEulerAngle(dt);nowQ=nowQ.Multiply(dQ);count++;if (count>1000){EulerAngle nowG=nowQ.ToEulerAngle();Serial.print(nowG.fRoll/3.1415926535*180,11);//横滚Serial.print(",");Serial.print(nowG.fPitch/3.1415926535*180,11);//俯仰Serial.print(",");Serial.print(nowG.fYaw/3.1415926535*180,11);//偏航Serial.print(",");Serial.print(nowQ.getS(),11);//偏航Serial.println();count=0;}}四元数与欧拉角之间的转换在3D图形学中，最常用的旋转表示方法便是四元数和欧拉角，比起矩阵来具有节省存储空间和方便插值的优点。
MATLAB在飞行器设计与控制中的应用实例
MATLAB在飞行器设计与控制中的应用实例一、引言飞行器设计与控制是航空航天领域的重要研究方向。
随着计算机和数值模拟技术的发展，MATLAB逐渐成为了飞行器设计与控制中不可或缺的工具。
本文将从飞行器设计与控制的角度，探讨MATLAB在该领域的应用实例。
二、飞行器设计1.1 飞行器气动力学模拟在飞行器设计中，了解飞行器的气动特性至关重要。
通过MATLAB中的气动力学模拟工具，可以快速准确地分析飞行器在不同工况下的气动力学性能。
例如，可以通过输入飞行器的几何参数和流场压力分布，利用MATLAB进行气动力学建模和仿真分析，提取重要气动参数，如升力、阻力和气动力矩。
这些参数对于飞行器的设计和性能评估具有重要意义。
1.2 飞行器结构优化设计飞行器结构优化设计是提高飞行器性能和降低结构质量的重要手段。
MATLAB 提供了优秀的优化工具箱，可以方便地进行设计参数的优化计算。
例如，可以通过建立结构静力学模型，同时考虑结构刚度、强度和质量等指标，在约束条件下求解最优解，得到飞行器结构的最佳设计方案。
这不仅可以提高飞行器的性能，并且能够满足飞行器的设计需求和约束条件。
三、飞行器控制2.1 飞行器姿态控制飞行器姿态控制是飞行器稳定飞行和精确导航的关键技术之一。
MATLAB提供了丰富的控制工具箱，可以方便地设计和调试姿态控制系统。
例如，可以利用线性系统理论设计飞行器姿态控制器，通过状态空间模型和传递函数模型进行系统分析和控制设计。
同时，MATLAB还提供了自适应控制、鲁棒控制和模糊控制等高级控制方法，能够满足不同需求的姿态控制任务。
2.2 飞行器轨迹规划与导航飞行器轨迹规划与导航是飞行器航行路径的设计与控制。
通过MATLAB中的路径规划与导航工具包，可以方便地规划飞行器的航行路径，并进行导航控制。
例如，可以利用优化算法和路径搜索算法，根据飞行器的起始点和目标点，计算出最优的飞行路径。
同时，利用导航控制算法，可以实时调整飞行器的航向和高度，保证飞行器按照预定轨迹安全准确地飞行。
matlab在飞行器制导控制系统研制中的应用
matlab在飞行器制导控制系统研制中的应用MATLAB在飞行器制导控制系统研制中具有广泛的应用。
以下是几个方面的应用：
1)动力学建模和仿真：MATLAB可以用于建立飞行器的动
力学模型，并进行仿真以评估其飞行性能。
通过使用MATLAB的Simulink工具箱，可以构建复杂的飞行器模型，包括飞行器的运动方程、传感器模型和控制器模型。
2)控制系统设计：MATLAB提供了丰富的工具和函数，用
于设计和分析飞行器的控制系统。
可以使用MATLAB的控制系统工具箱来设计各种类型的控制器，如PID控制器、线性二次调节器（LQR）和模糊控制器等。
通过系统分析工具，可以评估控制系统的稳定性、性能和鲁棒性。
3)导航和姿态估计：MATLAB可以用于飞行器的导航和姿
态估计。
通过使用传感器数据（如加速度计、陀螺仪和磁力计）来实现导航和姿态估计算法。
MATLAB提供了许多用于信号处理、过滤和状态估计的函数和工具箱。
4)自主飞行控制：MATLAB可以用于开发自主飞行控制算
法。
通过结合传感器数据和导航算法，可以实现飞行器的自主控制和路径规划。
MATLAB提供了机器学习和优化工具箱，可以用于开发自适应控制算法和优化飞行性能。
飞机惯性导航Matlab语言实现
%这是研究惯性导航的最好代码。
记得自己添加测试数据% 此为基于四元素法，角增量法的捷连惯导系统程序算法% > 飞行器飞行过程中飞行高度不变% > 航向角以逆时针为正% > 以地理系为导航坐标系% > 运行程序时需导入比力信息及陀螺议角速率信息clcclearclose allData = load('Data1.txt');f_INS = Data(:,2:4);% 加载加表数据wib_INS = Data(:,5:7);% 加载陀螺数据L0 = size(Data,1);Wie = 7.292115147e-5; %> 地球自传角速度Re = 6378245; %> 地球椭球长半径h = 30;% > 飞行高度e = 1/298.3;%> 初始经纬度Lamda(1) = 116.344695283*pi/180;% > 初始经度（弧度）L(1) = 39.975172*pi/180;% > 初始纬度（弧度）%> 初始姿态角Seita(1) = 0.120992605*pi/180; %> 俯仰角（弧度）Gama(1) = 0.010445947*pi/180; %> 横滚角（弧度）Ksai(1) = 91.637207*pi/180;% > 航向角（弧度）%> 初始速度Vx(1) = 0.000048637; %> x通道速度Vy(1) = 0.000206947;% > y通道速度Vz(1) = 0.007106781; %> z通道速度%> 重力加速度计算参数g0 = 9.7803267714;gk1 = 0.00193185138639;gk2 = 0.00669437999013;Vx = zeros(1,L0);Vy = zeros(1,L0);Vz = zeros(1,L0);Lamda = zeros(1,L0);L = zeros(1,L0);Seita = zeros(1,L0);Gama = zeros(1,L0);Ksai = zeros(1,L0); %> 四元素初始值e0 = cos(0.5*Ksai(1))*cos(0.5*Seita(1))*cos(i0.5*Gama(1))-sin(0.5*Ksai(1))*sin(0.5*Seita(1))*sin(0.5* Gama(1));e1 = -cos(0.5*Ksai(1))*sin(0.5*Seita(1))*cos(0.5*Gama(1))+sin(0.5*Ksai(1))*cos(0.5*Seita(1))*sin(0.5* Gama(1));e2 = -cos(0.5*Ksai(1))*cos(0.5*Seita(1))*sin(0.5*Gama(1))-sin(0.5*Ksai(1))*sin(0.5*Seita(1))*cos(0.5* Gama(1));e3 = cos(0.5*Ksai(1))*sin(0.5*Seita(1))*sin(0.5*Gama(1))+sin(0.5*Ksai(1))*cos(0.5*Seita(1))*cos(0.5* Gama(1));Ctb = [e0^2+e1^2-e2^2-e3^2 2*(e1*e2+e0*e3) 2*(e1*e3-e0*e2); %> 用四元素表示得姿态矩阵2*(e1*e2-e0*e3) e0^2-e1^2+e2^2-e3^2 2*(e2*e3+e0*e1);2*(e1*e3+e0*e2) 2*(e2*e3-e0*e1) e0^2-e1^2-e2^2+e3^2];E = [e0 e1 e2 e3]';%> 四元素的四个元素值for i = 1:L0f_INSc = f_INS(i,:)';wib_INSc = wib_INS(i,:)';Ry(i) = Re*(1-2*e+3*e*(sin(L(i)))^2);% > 计算子午圈主曲率半径Rx(i) = Re*(1+e*(sin(L(i)))^2); %> 计算卯酉圈主曲率半径g = g0*(1+gk1*(sin(L(i)))^2)*(1-2*h/Re)/sqrt(1-gk2*(sin(L(i)))^2);% > 重力加速度计算Cbt = Ctb';f_t = Cbt*f_INSc;% > 将体轴系中的比例转化到地理系Vx(i) = (f_t(1)+2*Wie*sin(L(i))*Vy(i)+Vx(i)*Vy(i)*tan(L(i))/Rx(i))/80+Vx(i);% > x通道速度计算Vy(i) = (f_t(2)-2*Wie*sin(L(i))*Vx(i)-Vx(i)*Vx(i)*tan(L(i))/Rx(i))/80+Vy(i);% > y通道速度计算Vz(i) = (f_t(3)+2*Wie*cos(L(i))*Vx(i)+Vx(i)*Vx(i)/Rx(i)+Vy(i)*Vy(i)/Ry(i)-g)/80+Vz(i);% > z通道速度计算Lamda(i) = Vx(i)/cos(L(i))/Rx(i)/80+Lamda(i);% > 经度计算if Lamda(i)>piLamda(i) = Lamda(i)-2*pi;% >经度在-180度(西经)到180(东经)范围endL(i) = Vy(i)/Ry(i)/80+L(i); %> 纬度计算if L(i)>(pi/2)L(i) = pi-L(i);% >纬度小于90度(北纬)endWetx_t(i) = -Vy(i)/Ry(i);Wety_t(i) = Vx(i)/Rx(i);Wetz_t(i) = Vx(i)*tan(L(i))/Rx(i);% > 在地理坐标系的位移角速率Wet_t = [Wetx_t(i) Wety_t(i) Wetz_t(i)]'; %> 在地理坐标系的位移角速率Wib_b = [wib_INSc(1) wib_INSc(2) wib_INSc(3)]';% > 陀螺仪测的角速率值Wie_t = [0 Wie*cos(L(i)) Wie*sin(L(i))]'; %> 在地理坐标系的地球角速率Wtb_b = Wib_b-Ctb*(Wie_t+Wet_t); %> 姿态矩阵角速率%> 用角增量法计算四元素姿态矩阵Mwtb = [0 -Wtb_b(1) -Wtb_b(2) -Wtb_b(3);Wtb_b(1) 0 Wtb_b(3) -Wtb_b(2);Wtb_b(2) -Wtb_b(3) 0 Wtb_b(1);Wtb_b(3) Wtb_b(2) -Wtb_b(1) 0]/80;derta = sqrt((Mwtb(1,2))^2+(Mwtb(1,3))^2+(Mwtb(1,4))^2);E = [eye(4)*(1-derta^2/8+derta^4/384)+(1/2-derta^2/48)*Mwtb]*E;% > E = (cos(0.5*derta)*eye(4)+Mwtb*sin(0.5*derta)/derta)*E，采用四阶近似算法e0 = E(1);e1 = E(2);e2 = E(3);e3 = E(4);Ctb = [e0^2+e1^2-e2^2-e3^2 2*(e1*e2+e0*e3) 2*(e1*e3-e0*e2);% > 用四元素表示得姿态矩阵2*(e1*e2-e0*e3) e0^2-e1^2+e2^2-e3^2 2*(e2*e3+e0*e1);2*(e1*e3+e0*e2) 2*(e2*e3-e0*e1) e0^2-e1^2-e2^2+e3^2];%> 姿态角计算Seita(i) = asin(Ctb(2,3)); %> 俯仰角计算Gama(i) = atan(-Ctb(1,3)/Ctb(3,3)); %> 横滚角计算if abs(Ctb(3,3))>epsGama(i) = atan(-Ctb(1,3)/Ctb(3,3));if Ctb(3,3)>0Gama(i) = Gama(i);elseif -Ctb(1,3)> 0Gama(i) = Gama(i)+pi;else Gama(i) = Gama(i)-pi;endelseif -Ctb(1,3)> 0Gama(i) = pi/2;else Gama(i) = -pi/2;endKsai(i) = atan(Ctb(2,1)/Ctb(2,2));% > 航向角计算if abs(Ctb(2,2))>epsKsai(i) = atan(Ctb(2,1)/Ctb(2,2));if Ctb(2,2)>0Ksai(i) = Ksai(i);elseif Ctb(2,1)> 0Ksai(i) = Ksai(i)+pi;else Ksai(i) = Ksai(i)-pi;endelseif Ctb(2,1)>0Ksai(i) = pi/2;else Ksai(i) = -pi/2;endend%> 将弧度换算为角度Seita = Seita*180/pi;Gama = Gama*180/pi;Ksai = Ksai*180/pi;L = L*180/pi;Lamda = Lamda*180/pi;t = 0.01:0.01:L0*0.01;%> 绘制曲线图figureplot(L,Lamda)% > 绘制经度变化曲线图grid onXlabel('经度');Ylabel('维度');title('经维度变化曲线图');figureplot(t,Seita)% > 绘制俯仰角变化曲线图grid onXlabel('时间/秒');Ylabel('俯仰角Seita/度');title('俯仰角变化曲线图');figureplot(t,Gama)% > 绘制横滚角变化曲线图grid onXlabel('时间/秒');Ylabel('横滚角Gama/度');title('横滚角变化曲线图'); figureplot(t,Ksai) %> 绘制航向角变化曲线grid onXlabel('时间/秒');Ylabel('航向角Ksai/度');title('航向角变化曲线图'); figureplot(t,Vx)% > 绘制东向速度变化曲线grid onXlabel('时间/秒');Ylabel('东向速度Vx 米/秒');title('东向速度变化曲线图'); figureplot(t,Vy)% > 绘制北向速度变化曲线grid onXlabel('时间/秒');Ylabel('北向速度Vy 米/秒');title('北向速度变化曲线图'); figureplot(t,Vz)% > 绘制垂直速度变化曲线grid onXlabel('时间/秒');Ylabel('垂直速度Vz 米/秒');title('垂直速度变化曲线图');。
捷联惯性导航系统的姿态算法
收稿日期 : 2003207225 基金项目 : 国防预研资助项目 (18 YXGFK D118) 作者简介 : 刘 危 (1974 - ) ,男 ,江西都昌人 ,博士生 , bullkill @21cn. com.
© 1994-2007 China Academic Journal Electronic Publishing House. All rights reserved.
2005 年 1 月 第 31 卷 第 1 期
北京航空航天大学学报 Journal of Beijing University of Aeronautics and Astronautics
January 2005 Vol. 31 No11
捷联惯性导航系统的姿态算法
刘 危 解旭辉 李圣怡
( 19)
Δ T ,则近似中的算法误差为 若令 λ= Ω Δ δ ^Φ - δ Φ ≈δ Φ =
N
- 2 ( p + N)
2
p + N +1
∏
( p + N) ! 2 ( p + N ) +1 λ ab p + N +1 ( 2 k - 1) k =1
( 13)
A =
… … … … … … … A i ,1 … A i , N - 1 A i , N … A i , N + p - 1 A i , N + p … … … … …
第 1 期 刘 危等 : 捷联惯性导航系统的姿态算法
47
k1 K( N + p - 1) × 1 = k2
d1
其中 , A ij ( j ≠( N + p) ) 的定义如式 ( 11) ; 而 A i , N + P 的定义如下 :
- 1、下载文档前请自行甄别文档内容的完整性，平台不提供额外的编辑、内容补充、找答案等附加服务。
- 2、"仅部分预览"的文档,不可在线预览部分如存在完整性等问题,可反馈申请退款(可完整预览的文档不适用该条件!)。
- 3、如文档侵犯您的权益，请联系客服反馈,我们会尽快为您处理(人工客服工作时间：9:00-18:30)。
捷联惯导程序，依据加表和陀螺仪的输出数据来求解飞行器的姿态clc;clear;format long; %设置数据精度为15位小数Data=importdata(''); % 导入实验所采集的数据，以矩阵形式赋给Data变量，必须与该M文件在同一个文件夹中Px=Data(:,3); % Px,Py,Pz为陀螺仪的输出值Py=Data(:,4);Pz=Data(:,5);Nx=Data(:,6); % Nx,Ny，Nz为加速度计的输出值Ny=Data(:,7);Nz=Data(:,8);% 陀螺仪模型参数标定如下：Sx = ; Sy = ; Sz = ;Mxy = ; Mxz = ; Myx = ;Myz = ; Mzx = ; Mzy = ;Dx = ; Dy = ; Dz = ;GyroCali_A = [ 1 -Mxy -Mxz ; -Myx 1 -Myz ; -Mzx -Mzy 1 ];% 加速度计模型参数标定如下：Kx = ; Ky = ; Kz = ;Ixy = ; Ixz = ; Iyx = ;Iyz = ; Izx = ; Izy = ;Bx = ; By = ; Bz = ;AccCali_A = [1 -Ixy -Ixz ; -Iyx 1 -Iyz ; -Izx -Izy 1 ];Delta_t = ; %采样时间为秒Delta_Theta_x = 0;Delta_Theta_y = 0;Delta_Theta_z = 0; %定义陀螺仪输出的角度增量Delta_Vx = 0;Delta_Vy = 0;Delta_Vz = 0; %定义加速度计输出的速度增量L = zeros(1,12001);L(1)= *pi/180 ; %纬度用L表示，纬度的初始值划为弧度形式，因为后面计算位置矩阵更新L(2)= *pi/180 ; %时需要用到前两次的L值来计算当前L值，所以在此定义2个初始L值Lamda = *pi/180 ; %经度用Lamda表示，经度的初始值划为弧度形式h = 136 ; %高度用h表示V = [ 0 ; 0 ; 0 ]; %导航坐标系中的东北天初始速度都为0Vx = 0; %方便后面的速度计算与速度更新Vy = 0;Vz = 0;Theta = 0;Gama = 0;Fai = 0; %初始姿态角（俯仰角/倾斜角/航向角）都为0,此处均为弧度Re = 6378254 ; Rp = 6356803 ;%定义地球的半长轴与半短轴e = (Re - Rp)/Re ; %定义旋转椭球扁率（椭球度）Wie = 180*pi ; %定义地球自转角速度，地球坐标系相对于惯性坐标系的角速度Theta_Matrix = zeros(1,12000); %定义姿态角矩阵，供画图用Gama_Matrix = zeros(1,12000);Fai_Matrix = zeros(1,12000);L_Matrix = zeros(1,12001); %定义经纬度矩阵，供画图用，L的特殊性决定了其数据个数为12001L_Matrix(1) = ;Lamda_Matrix = zeros(1,12000);Ve_Matrix = zeros(1,12000); %定义速度矩阵，供画图用Vn_Matrix = zeros(1,12000);Vu_Matrix = zeros(1,12000);%以下计算捷联矩阵的初始值，捷联矩阵的初始值仅仅由Theta，Gama，Fai的初始值决定T = [ cos(Gama)*cos(Fai)-sin(Gama)*sin(Theta)*sin(Fai) -cos(Theta)*sin(Fai)sin(Gama)*cos(Fai)+cos(Gama)*sin(Theta)*sin(Fai) ;cos(Gama)*sin(Fai)+sin(Gama)*sin(Theta)*cos(Fai)cos(Theta)*cos(Fai) sin(Gama)*sin(Fai)-cos(Gama)*sin(Theta)*cos(Fai) ;-sin(Gama)*cos(Theta) sin(Theta) cos(Gama)*cos(Theta) ];%由捷联矩阵的初始值计算初始四元数值，为捷联矩阵的实时更新做准备if(T(3,2)-T(2,3)>0)Q1 = *sqrt(1+T(1,1)-T(2,2)-T(3,3));else if (T(3,2)-T(2,3)==0)Q1 = 0;else Q1 = *sqrt(1+T(1,1)-T(2,2)-T(3,3));end %求解Q1endif(T(1,3)-T(3,1)>0)Q2 = *sqrt(1-T(1,1)+T(2,2)-T(3,3));else if (T(1,3)-T(3,1)==0)Q2 = 0;else Q2 = *sqrt(1-T(1,1)+T(2,2)-T(3,3));end %求解Q2endif(T(2,1)-T(1,2)>0)Q3 = *sqrt(1-T(1,1)-T(2,2)+T(3,3));else if (T(2,1)-T(1,2)==0)Q3 = 0;else Q3 = *sqrt(1-T(1,1)-T(2,2)+T(3,3));end %求解Q3endQ0 = *sqrt(1-Q1*Q1-Q2*Q2-Q3*Q3); %求解Q0Q = [Q0 ; Q1 ; Q2 ; Q3]; %四元数初始值Q = Q / sqrt(Q0*Q0+Q1*Q1+Q2*Q2+Q3*Q3); %四元数的初始归一化，为得到最小漂移误差%以下求位置矩阵的初始值，通过位置矩阵更新后，反过来算运载体所在的经纬度%位置矩阵仅仅与经纬度有关系，Ce2n表示把地球坐标系转换为导航坐标系的转换矩阵Ce2n = [ -sin(Lamda) cos(Lamda) 0 ;-sin( L(1) )*cos(Lamda) -sin(L(1))*sin(Lamda) cos( L(1) );cos( L(1) )*cos(Lamda) cos( L(1) )*sin(Lamda) sin( L(1) ) ];%大循环，共执行12000次，实时更新捷联矩阵，速度矩阵，位置矩阵，保存作图所需数据for k = 1:12000;GyroCali_B = [Sx*Px(k)-Dx*Delta_t ; Sy*Py(k)-Dy*Delta_t ;Sz*Pz(k)-Dz*Delta_t ];Delta_Theta = GyroCali_A * GyroCali_B ; %计算陀螺仪输出的角度增量Delta_Theta_x = Delta_Theta(1);Delta_Theta_y = Delta_Theta(2);Delta_Theta_z = Delta_Theta(3);Delta_Theta_Module = sqrt( Delta_Theta_x * Delta_Theta_x + Delta_Theta_y* Delta_Theta_y + Delta_Theta_z * Delta_Theta_z );AccCali_B = [Kx*Nx(k)-Bx*Delta_t ; Ky*Ny(k)-By*Delta_t ;Kz*Nz(k)-Bz*Delta_t ];Delta_V = AccCali_A * AccCali_B ; %计算加速度计输出的速度增量Delta_Vx = Delta_V(1);Delta_Vy = Delta_V(2);Delta_Vz = Delta_V(3);Delta_V_Module = sqrt( Delta_Vx * Delta_Vx + Delta_Vy * Delta_Vy + Delta_Vz* Delta_Vz );%使用毕卡法求解四元数更新矩阵，即捷联矩阵Bika = zeros(4);Bika(1,1) = cos * Delta_Theta_Module);Bika(1,2) = -Delta_Theta_x / Delta_Theta_Module * sin * Delta_Theta_Module);Bika(1,3) = -Delta_Theta_y / Delta_Theta_Module * sin * Delta_Theta_Module);Bika(1,4) = -Delta_Theta_z / Delta_Theta_Module * sin * Delta_Theta_Module);Bika(2,1) = -Bika(1,2);Bika(2,2) = Bika(1,1);Bika(2,3) = -Bika(1,4);Bika(2,4) = Bika(1,3);Bika(3,1) = -Bika(1,3);Bika(3,2) = -Bika(2,3);Bika(3,3) = Bika(1,1);Bika(3,4) = -Bika(1,2);Bika(4,1) = -Bika(1,4);Bika(4,2) = -Bika(2,4);Bika(4,3) = -Bika(3,4);Bika(4,4) = Bika(1,1);Q = Bika * Q; % 每循环一次，更新一次四元素Q值，为求捷联矩阵Q = Q / sqrt(Q0*Q0+Q1*Q1+Q2*Q2+Q3*Q3); %四元数的归一化，为得到最小漂移误差Q0 = Q(1);Q1 = Q(2);Q2 = Q(3);Q3 = Q(4);%捷联矩阵的四元数表达式T = [ Q0*Q0+Q1*Q1-Q2*Q2-Q3*Q3 2*(Q1*Q2-Q0*Q3) 2*(Q1*Q3+Q0*Q2) 2*(Q1*Q2+Q0*Q3) Q0*Q0-Q1*Q1+Q2*Q2-Q3*Q3 2*(Q2*Q3-Q0*Q1)2*(Q1*Q3-Q0*Q2) 2*(Q2*Q3+Q0*Q1) Q0*Q0-Q1*Q1-Q2*Q2+Q3*Q3 ];%*********************************************************************%********************求三个姿态角Theta，Gama和 Fai ********************%*********************************************************************Theta_Main = asin( T(3,2) );Gama_Main = atan( -T(3,1) / T(3,3));Fai_Main = atan( -T(1,2) / T(2,2));Theta = Theta_Main;if (T(3,3)>0)Gama = Gama_Main ;else if (T(3,3)<0 && Gama_Main > 0)Gama = Gama_Main + pi;else Gama = Gama_Main - pi; %此处用else实为不妥，不过为了程序的完善性，只能这样了endendif ( T(2,2)<0 )Fai = Fai_Main + pi ;else if (T(2,2)==0)Fai = pi/2;else if ( Fai_Main>0)Fai = Fai_Main;else Fai = Fai_Main + 2*pi ;endendend%以下存储姿态角到三个矩阵里面，为画图做准备Theta_Matrix(k) = Theta*180/pi; %作图用矩阵,以角度表示Gama_Matrix(k) = Gama*180/pi; %作图用矩阵,以角度表示if (Fai<2*pi)Fai_Matrix(k) = Fai*180/pi;else Fai_Matrix(k) = Fai*180/pi-360; %作图用矩阵,以角度表示end%到此为止，姿态角的求解完毕，以下先求速度%********************************************************************* %********************求飞行器相对于东北天的速度*************************%********************************************************************* Rm = Re*( 1-2*e+3*e*sin( L(k+1) )^2 );Rn = Re*( 1+e*sin(L(k+1))^2 );LL = 3/2*L(k+1) - 1/2*L(k) ;F = [ 0 -1/(Rm + h) 0 ; 1/(Rn + h) 0 0 ; tan( LL )/(Rn + h) 0 0];g = +*sin(L(k+1))^*h;G = [0;0;-g];Wen2n = F*V;Wie2n = [0 ; Wie*cos(L(k+1)); Wie*sin(L(k+1))];W = 2*Wie2n + Wen2n;W_X = [ 0 -W(3) W(2) ; W(3) 0 -W(1) ; -W(2) W(1) 0 ]; %此式中W_X为2*Wie+Wen2n 的反对称矩阵V = V + T*( Delta_V+ [ 0 -Delta_Theta_z Delta_Theta_y ; Delta_Theta_z 0 -Delta_Theta_x ; -Delta_Theta_y Delta_Theta_x 0 ]*Delta_V ) + Delta_t*(G-W_X*V);Vx = V(1);Vy = V(2);Vz = V(3);Ve_Matrix(k) = V(1);Vn_Matrix(k) = V(2);Vu_Matrix(k) = V(3);%********************************************************************* %********************求飞行器所在的经纬度****************************** %*********************************************************************Epsilon = F*V*Delta_t;Ce2n =( eye(3) - [ 0 -Epsilon(3) Epsilon(2) ; Epsilon(3) 0 -Epsilon(1) ; -Epsilon(2) Epsilon(1) 0 ] )*Ce2n ; %位置矩阵实时更新%下面通过位置矩阵来实时更新经纬度L(k+2) = asin( Ce2n(3,3)); %由于L本来就是以矩阵形式定义的，下面定义一个把L用角度表示的矩阵L_Matrix(k+1) = L(k+2)*180/pi;Lamda_Main = atan( Ce2n(3,2)/Ce2n(3,1) ); %计算出来的L和Lamda都是弧度制的if (Ce2n(3,1)>0)Lamda = Lamda_Main;else if (Lamda_Main<0)Lamda = Lamda_Main + pi;else Lamda = Lamda_Main - pi;endendLamda_Matrix(k) = Lamda*180/pi; %作图用矩阵，以角度表示end%************************************************************************** %********************以下是画图程序*****************************************%************************************************************************** k=1:1:12000; %绘制三轴姿态变化图线-绿色figure(1);plot(k/20,Theta_Matrix(k),'g');xlabel('Time(second)');ylabel('Angle(degree)');title('Theta(俯仰角)');grid on;figure(2);plot(k/20,Gama_Matrix(k),'g');xlabel('Time(second)');ylabel('Angle(degree)');title('Gama（滚转角）');grid on;figure(3);plot(k/20,Fai_Matrix(k),'m');xlabel('Time(second)');ylabel('Angle(degree)');title('Fai（偏航角）');grid on;%绘制东北天个方向的速度变化曲线-红色figure(4);plot(k/20,Ve_Matrix(k),'r');xlabel('Time(second)');ylabel('Velocity(m/s)');title('Ve（东向速度）');grid on;figure(5);plot(k/20,Vn_Matrix(k),'r');xlabel('Time(second)');ylabel('Velocity(m/s)');title('Vn（北向速度）');grid on;figure(6);plot(k/20,Vu_Matrix(k),'r');xlabel('Time(second)');ylabel('Velocity(m/s)');title('Vu（天向速度）');grid on;%绘制飞行器所在经纬度曲线-蓝色figure(7);plot(k/20,L_Matrix(k),'b');xlabel('Time(second)');ylabel('Degree');title('Latitude L(纬度)');grid on;figure(8);plot(k/20,Lamda_Matrix(k),'b'); xlabel('Time(second)');ylabel('Degree');title('Longitude Lamda(经度)'); grid on;。