之前将高成本的捷联惯导忽略地球自转、圆锥曲线运动以及划桨运动等化简为可适用低成本的捷联惯导MATLAB程序，但是其中的子程序都是用的严老师的代码，想自己锻炼一下自己的代码能力，所以对低成本的捷联惯导进行重新编写！！

一、姿态角转换为四元数

  对于单位四元数的三角函数表示，有：

$$\mathbf{Q}=q_0+{\mathbf{q}}_v=\cos \frac{\varphi }{2}+\mathbf{u}\sin \frac{\varphi }{2}$$
（1）航向角$\psi$，运载体纵轴在当地水平面上的投影线与当地地理北向的夹角，常取北偏东为正，即若从空中俯视运载体，地理北向顺时针旋转至纵轴水平投影线的角度，角度范围为0～360° ；
（2）俯仰角$\theta$，运载体纵轴与其水平投影线之间的夹角，当运载体抬头时角度定义为正，角度范围-90°～90°；
（3）横滚角$\gamma$，运载体立轴与纵轴所在铅垂面之间的夹角，当运载体向右倾斜时角度定义为正，角度范围-180°～180°。
  虽然航向角$\psi$习惯上常定义为北偏东为正，但是当定义导航坐标系为“东-北-天”地理坐标系时，航向角在绕天向轴转动时不符合右手规则。为了符合右手规则和推导公式简洁对称，除非特别说明，本节将航向角定义为北偏西为正，且取值范围为 $(-\pi ，\pi ]$，这是在后续阅读相关公式时需要特别注意的。当然，如果要将相关公式应用于北偏东的航向角，只需再增加一个简单的航向角转换过程即可。
在“东-北-天312”欧拉角定义下，由欧拉角求解四元数的公式为：

程序验证：

%***************************************
% 将欧拉角转为四元数：att2que(att)
% input：att=[pitch;roll;yaw];           unit：rad
% output：qnb=[q0;q1;q2;q3];             q0为实部
%***************************************
theta = 0; gamma = 0; psi = 90*pi/180;
att = [theta;gamma;psi];
s1 = sin(theta/2); s2 = sin(gamma/2); s3 = sin(psi/2);
c1 = cos(theta/2); c2 = cos(gamma/2); c3 = cos(psi/2);
qnb= [c3*c1*c2 - s3*s1*s2;c3*s1*c2 - s3*c1*s2;s3*s1*c2 - c3*c1*s2;s3*c1*c2 - c3*s1*s2;]

结果：

>> exe0.7071000.7071

改写为子程序备用：

function att2que qnb = (att);
att = [theta;gamma;psi];
s1 = sin(theta/2); s2 = sin(gamma/2); s3 = sin(psi/2);
c1 = cos(theta/2); c2 = cos(gamma/2); c3 = cos(psi/2);
qnb= [c3*c1*c2 - s3*s1*s2;c3*s1*c2 - s3*c1*s2;s3*s1*c2 - c3*c1*s2;s3*c1*c2 - c3*s1*s2;];

二、四元数转换为姿态角

  由四元数直接求解欧拉角并不容易。实际上，可通过姿态阵作为中间过渡量，先由四元数计算姿态阵，再由姿态阵计算欧拉角，综合一起后其结果为：

function att = que2att(qnb)
%***************************************
% 四元数转换为姿态角：que2att(qnb)
% input：qnb=[q0;q1;q2;q3];             q0为实部       
% output：att=[pitch;roll;yaw];           unit：rad     
%***************************************
att(1) = asin(2*(qnb(3)*qnb(4) + qnb(1)*qnb(2)));
if abs(2*(qnb(3)*qnb(4) + qnb(1)*qnb(2)) <= 0.999999att(2) = -atan2*(2*(qnb(2)*qnb(4) - qnb(1)*qnb(3))),qnb(1)^2 - qnb(2)^2 - qnb(3)^2 + qnb(4)^2);att(3) = -atan2*(2*(qnb(2)*qnb(3) - qnb(1)*qnb(4))),qnb(1)^2 - qnb(2)^2 + qnb(3)^2 - qnb(4)^2);
elseatt(2) = -atan2*(2*(qnb(2)*qnb(4) + qnb(1)*qnb(3))),qnb(1)^2 + qnb(2)^2 - qnb(3)^2 - qnb(4)^2);att(3) = 0;
end	

三、反对称阵

引入三维向量的反对称阵概念后，两向量之间叉乘运算可等价表示为前一向量的反对称阵与后一向量之间的矩阵乘法运算，亦即：

funcion ssm = skew(v)
%***************************************
% 求三维向量的反对称阵（skew symmetric matrix）：skew(v)
% input：v = [v_x;v_y;v_z];          
% output：ssm = [ssm_x;ssm_y;ssm_z];             
%***************************************
ssm = [0,-v(3),v(2);v(3),0,-v(1);-v(2),v(1),0];

四、位置更新

f = 1/298.257223563;
R_e = 6.378136998405e6;
e = sqrt(2f-f^2);
R_N = R_e/(1-e^2*(sin(pos(1)))^2)^0.5;
R_M = R_N*(1-e^2)/(1-e^2*(sin(pos(1)))^2);
R_Mh = R_M + pos(3);
R_Nh = R_N + pos(3);
M_pv = [0,1/R_Mh,0;sec(pos(1))/R_Nh,0,0;0,0,1];vn1 = (vn2+vn1)/2;
pos = pos + M_pv*vn1*nts;	    

五、姿态更新

wie = 7.2921151467e-5;  
wnie = [0,wie*cos(pos(1)),wie*sin(pos(1))]';
wnen = vn.*[-1/(R_M + pos(3));1/(R_N + pos(3));tan(pos(1))/(R_N + pos(3))];
wnin = wnie + wnen;
% 姿态更新
q_bb = [cos(norm(delta_theta_m)/2);(delta_theta_m/norm(delta_theta_m))'*sin((norm(delta_theta_m)/2))];
qnb = qmul(rv2q(-wnin*nts), qmul(qnb, q_bb));  
qnb = qnormlz(qnb);

六、程序及数据

主程序：

clc;
clear;
imu = load ('imu.mat');                                %imu数据： avp=[wm, vm, tt(2:end)]; gx(rad)  gy  gz ax (m/s)  ay  az time  
trj = load ('trj.mat');                                %trj=[att, vn, pos]; att(度) vn(米每秒) pos(米)
deg2rad = pi/180;
att = [0;0;90]*deg2rad; vn = [0;0;0]; pos = [[34;108]*deg2rad;100];
qnb = att2que(att);
n = 1;t = 0;ts = 0.01;len = length(imu.avp);avps = zeros(len,10);
for k = 1:n:lent = t + ts;wm = imu.avp(k,1:3);vm = imu.avp(k,4:6);[qnb,vn,pos] = my_insupdate(qnb,vn,pos,wm,vm,ts);cnb = que2att(qnb);cnb(3) = mod(cnb(3),2*pi);avps(k,:) = [cnb';vn;pos;t]';
end%姿态
tt=length(avps);
tf=length(trj.trj);
figure(1);
subplot(321);
plot(1:tt,(avps(1:tt,1:2)/deg2rad),1:tf,(trj.trj(1:tf,1:2)/deg2rad),'--'); title('俯仰角和横滚角');xlabel('d');ylabel('\theta,\gamma(\circ)');
legend('\it\theta','\it\gamma','\bf\theta','\bf\gamma');grid on;
subplot(322);
plot(1:tt,(avps(1:tt,3)/deg2rad),1:tf,(trj.trj(1:tf,3)/deg2rad),'--'); title('偏航角');xlabel('d');ylabel('\Phi(\circ)');
legend('\it\Phi','\bf\Phi') ;grid on;
subplot(323);
plot(1:tt,(avps(1:tt,4:5)),1:tf,(trj.trj(1:tf,4:5)),'--'); title('速度');xlabel('d');ylabel('Vel/(m.s^{-1})');
legend('\itv\rm_E','\itv\rm_N','\bfv\rm_E','\bfv\rm_N');grid on;
subplot(324);
plot(1:tt,(avps(1:tt,6)),1:tf,(trj.trj(1:tf,6)),'--'); title('速度');xlabel('d');ylabel('Vel/(m.s^{-1})');
legend('\itv\rm_U','\bfv\rm_U');grid on;
subplot(325);
plot(1:tt,deltapos(avps(1:tt,7:9)),1:tf,deltapos(trj.trj(1:tf,7:9)),'--'); title('位置');xlabel('d');ylabel('\DeltaPos / m');
legend('\Delta\itL', '\Delta\it\lambda', '\Delta\ith','\Delta\bfL', '\Delta\bf\lambda', '\Delta\bfh');grid on;  

子程序：

function [qnb,vn2,pos]=my_insupdate(qnb,vn1,pos,delta_theta_m,delta_v_m,ts)
n=1;                                                                  
nts=n*ts;
g0=9.780325333434361;                                                  % 单位;m/s^2
sl = sin(pos(1));                                                      % pos(1)=L
sl2 = sl*sl;  sl4 = sl2*sl2;
gLh = g0*(1+5.27094e-3*sl2+2.32718e-5*sl4)-3.086e-6*pos(3); 
gn = [0;0;-gLh];f = 1/298.257223563;
R_e = 6.378136998405e6;
e = sqrt(2*f-f^2);
R_N = R_e/(1-e^2*sl2)^0.5;
R_M = R_N*(1-e^2)/(1-e^2*sl2);
R_Mh = R_M + pos(3);
R_Nh = R_N + pos(3);
M_pv = [0,1/R_Mh,0;sec(pos(1))/R_Nh,0,0;0,0,1];wie = 7.2921151467e-5;    
wnie = [0,wie*cos(pos(1)),wie*sin(pos(1))]';
wnen = vn1.*[-1/(R_M + pos(3));1/(R_N + pos(3));tan(pos(1))/(R_N + pos(3))];
wnin = wnie + wnen;%速度更新
vsf =q2mat(qnb)*(delta_v_m + 1/2*cross(delta_theta_m,delta_v_m))';      % avp2(4:6) = avp1(4:6) + vsf + gn*Tm;
vn2 = vn1 + vsf + gn*nts;
%位置更新
vn1 = (vn2+vn1)/2;
pos = pos + M_pv*vn1*nts;
% 姿态更新
q_bb = [cos(norm(delta_theta_m)/2);(delta_theta_m/norm(delta_theta_m))'*sin((norm(delta_theta_m)/2))];
qnb = qmul(rv2q(-wnin*nts), qmul(qnb, q_bb));  
qnb = qnormlz(qnb);
end

数据及完整程序

https://download.csdn.net/download/qq_38364548/87380265