-
捷联惯导程序，依据加表和陀螺仪的输出数据来求解飞行器的姿态
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
= -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<
/p>
表示，经度的初始值划为弧度形式
h
= 136
%
高度用
h
表示
V = [ 0 0
0 ];
%
导航坐标系中的东北天
初始速度都为
0
Vx = 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
的特殊性决定了其数
据个数为
12001
L_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
，
Ga
ma
，
Fai
的初始值决定
T
=
[
cos(Gama)*cos(Fai)-sin(Gam
a)*sin(Theta)*sin(Fai)
-cos(Theta)*sin(Fai)
sin(Gama)*cos(Fai)+cos(Gama)*sin(Theta)*sin(F
ai)
cos
(Gama)*sin(Fai)+sin(Gama)*sin(Theta)*cos(Fai)
cos(Theta)*cos(Fai)
sin(Gama)*sin(Fai)-cos(Gama)*sin(Theta)*cos(Fa
i)
-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
%
求解
Q1
end
if(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
%
求解
Q2
end
if(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
%
求解
Q3
end
Q0 =
0.5*sqrt(1-Q1*Q1-Q2*Q2-Q3*Q3);
%
求解
Q0
Q = [Q0 Q1 Q2 Q3];
%
四元数初始值
Q = Q / sqrt(Q0*Q0+Q1*Q1+Q2*Q2+Q3*Q3);
%
四元数的初始归一化，为得到最小漂移
误差
< br>
%
以下求位置矩阵的初始值，通过位
置矩阵更新后，反过来算运载体所在的经纬度
%
位置矩阵仅仅与经纬度有关系，
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) )
];
%
大循环，共执行
< br>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;
% <
/p>
每循环一次，更新一次四元素
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;
-
-
-
-
-
-
-
-
-
上一篇：数字媒体技术 复习资料
下一篇：空压机型号