019静态惯性器件捷联惯导仿真(1)

接下来博文对严恭敏老师《捷联惯导算法与组合导航原理讲义》中捷联惯导仿真的注释，为自己做一个备忘，也为其他像我一样刚入门的学习者提供一些便利。参考的公式内容也都出自上书中。上书可以到捷联惯导算法与组合导航原理讲义-严恭敏下载，如果你没有积分或者想保留自己的积分，也可以到004旋转矩阵系统理解文末下载。注：知识无价亦无界！

博文中函数注释的顺序根据主函数的调用次序排列。

1、定义全局变量

%全局变量

global Re ff wie g0 ug arcdeg arcmin arcsec hur dph dpsh ugpsHz

Re = 6378137;              % 地球半径
ff = 1/298.527;            % 地球扁率
wie = 7.2921151467e-5;     % 地球自转角速度
g0 = 9.7803267714;         % 重力加速度
ug = g0 * 1e-6;            % 微 g
arcdeg = pi/180;           % 角度
arcmin = arcdeg/60;        % 角分
arcsec = arcmin/60;        % 角秒
hur = 3600;                % 小时
dph = arcdeg/hur;          % 度/小时
dpsh = arcdeg/sqrt(hur);   % 度/sqrt(小时)
ugpsHz = ug/sqrt(1);       % ug/sqrt(Hz)

纯粹的定义，不进行说明。

2、姿态角转换为四元数

% 姿态角转换为四元数
% 参考(B-12)
function qnb = a2qua( att )   % att = [θ γ ψ];

s =sin(att/2);
c = cos(att/2);
si = s(1);                    % sinθ
sj = s(2);                    % sinγ
sk = s(3);                    % sinψ
ci = c(1);                    % cosθ
cj = c(2);                    % cosγ
ck = c(3);                    % cosψ

qnb = [
    ci*cj*ck - si*sj*sk;
    si*cj*ck - ci*sj*sk;
    ci*sj*ck + si*cj*sk;
    ci*cj*sk + si*sj*ck;
    ];

end

书中公式(B-12)可写作：
$Q^g_b= \left[ \begin{matrix} C_{\theta/2}C_{\gamma/2}C_{\psi/2} - S_{\theta/2}S_{\gamma/2}S_{\psi/2} \\ S_{\theta/2}C_{\gamma/2}C_{\psi/2} - C_{\theta/2}S_{\gamma/2}S_{\psi/2} \\ C_{\theta/2}S_{\gamma/2}C_{\psi/2} - S_{\theta/2}C_{\gamma/2}S_{\psi/2} \\ C_{\theta/2}C_{\gamma/2}S_{\psi/2} - S_{\theta/2}S_{\gamma/2}C_{\psi/2} \\ \end{matrix} \right]$
这样是不是变得很有规律。另外注意程序中对应关系：
$\begin{matrix} i \xrightarrow[]{} \theta \\ \\ j \xrightarrow[]{} \gamma \\ \\ k \xrightarrow[]{} \psi \\ \end{matrix}$

3、地球导航参数计算

% 地球导航参数计算

function eth = earth( pos, vn )

global Re ff wie g0           % 地球半径 地球扁率 地球自转角速率 重力加速度

ee = sqrt( 2*ff - ff^2 );     % 第一偏心率
e2 = ee^2;                    % 第一偏心率的平方 (3.1-7)

eth.sl = sin(pos(1));         % pos(1)为纬度，即求纬度正弦值
eth.cl = cos(pos(1));         % 纬度余弦值
eth.tl = eth.sl /eth.cl;      % 纬度正切值
eth.sl2 = eth.sl *eth.sl;     % 纬度正弦值的平方
sl4 = eth.sl2 *eth.sl2;       % 纬度正弦值的四次方

sq = 1 - e2*eth.sl2;          % 卯酉圈曲率半径分母的平方
sq2 = sqrt(sq);               % 卯酉圈曲率半径分母

eth.RMh = Re *(1-e2) /sq /sq2 +pos(3);   % 子午圈主曲率半径+飞行高度 (3.1-23)
eth.RNh = Re /sq2 +pos(3);               % 卯酉圈主曲率半径+飞行高度 (3.1-14)
eth.clRNh = eth.cl *eth.RNh;             % 可以理解为卯酉圈曲率半径加飞行高度的圆圈在飞机所处纬度平面的投影圆圈的半径
eth.wnie = wie * [0; eth.cl; eth.sl];    % 地球自转角速度(或地球相对于惯性系的角速度)在导航系中的投影 (4.1-3)
eth.vn = vn;
eth.wnen = [-vn(2)/eth.RMh; vn(1)/eth.RNh; vn(1)/eth.RNh*eth.tl];      % 飞机相对地球的旋转角速度在导航系中的投影 (3.1-14)或(4.1-4)
eth.wnin = eth.wnie +eth.wnen;                                         % 导航系相对于惯性系的角速度在导航系中的投影 P69底部
eth.wnien = eth.wnie +eth.wnin;                                        % 参见 (4.1-20)比力方程中角速度相加部分
gLh = g0 *(1 +5.27094e-3 *eth.sl2 +2.32718e-5 *sl4) -3.086e-6 *pos(3); % grs80重力模型
eth.gn = [0; 0; -gLh];                                                 % 重力矢量
eth.gcc = eth.gn - cross(eth.wnien, vn);                               % 参见 (4.1-20)比力方程中后两项

end

eth.wnin = eth.wnie +eth.wnen;  
eth.wnien = eth.wnie +eth.wnin;

对于这两行代码，
$\omega ^n_{in} = \omega ^n_{ie} + \omega^n_{en}$
$\omega ^n_{ien} = \omega ^n_{ie} + \omega^n_{in}$
参考公式(4.1-20),
$\dot{v}^n_{en}=C^n_bf^b_{sf} - (2 \omega ^n_{ie} + \omega^n_{en}) \times v^n_{en} + g^n$
则：
$\omega ^n_{ien} = 2 \omega ^n_{ie} + \omega^n_{en}$

eth.gcc = eth.gn - cross(eth.wnien, vn);

即：
$eth.gcc = g^n - (2 \omega ^n_{ie} + \omega^n_{en}) \times v^n_{en}$

gLh = g0 *(1 +5.27094e-3 *eth.sl2 +2.32718e-5 *sl4) -3.086e-6 *pos(3);

参考公式 (3.2-23)
$\begin{matrix} g_{Lh} = g_e(1 + \beta sin^2L - \beta _1 sin^22L ) - \beta_2h \\ \\ = g_e[1 + \beta sin^2L - 4\beta _1 sin^2L (1-sin^2L) ] - \beta_2h\\ \\ =g_e[1 + (\beta - 4\beta_1) sin^2L + 4\beta _1 sin^4L ] - \beta_2h\\ \end{matrix}$
但是根据grs80重力模型没有算出跟程序代码相同的系数。

4、四元数共轭

% 四元数共轭

function qout = qconj( qin )

qout = [ qin(1); -qin(2:4) ];  % 参考 (2.4-13)

end

这个应该不需要多说了吧0.0

5、四元数乘向量

% 四元数乘向量（三维向量的坐标变换）
% 参考 (2.4-25)
function vo = qmulv( q, vi )

qi = [0; vi];
qo = qmul( qmul(q, qi), qconj(q) );
vo = qo( 2:4, 1);

end

实质上就是将三维向量通过四元数进行坐标变换。至于qmul是什么，看下面。

6、四元数相乘

% 四元数相乘
% 参考公式 (2.4-6)

function q = qmul( q1, q2 )

q =[
    q1(1)*q2(1) - q1(2)*q2(2) - q1(3)*q2(3) - q1(4)*q2(4);
    q1(1)*q2(2) + q1(2)*q2(1) + q1(3)*q2(4) - q1(4)*q2(3);
    q1(1)*q2(3) + q1(3)*q2(1) + q1(4)*q2(2) - q1(2)*q2(4);
    q1(1)*q2(4) + q1(4)*q2(1) + q1(2)*q2(3) - q1(3)*q2(2);
    ];

end

7、四元数加失准角误差

% 四元数加失准角误差

function qpb = qaddphi( qnb, phi )

qpb = qmul(rv2q(-phi), qnb);   % 求添加失准角四元数，得到的机体系到导航系的四元数

end

对这一条，书中是这样解释的：

矩阵表示式(4.2-5)，即 $C^{n'}_b=C^{n'}_nC^n_b$ ，改写成四元数形式为 $Q^{n'}_b=Q^{n'}_n \circ Q^n_b$ 。由于从真是导航系（n系）到计算导航系（n’系）的失准角为 $\phi$ ，反之，从n’系到n系的失准角应为 $-\phi$ ，若将 $-\phi$ 视为等效旋转矢量，则与其等效的变换四元数为 $Q^n_{n'}$ 。上述程序中，变量 $qpb$ 、 $qnb$ 和 $phi$ 分别代表 $Q^{n'}_b$ 、 $Q^n_b$ 和 $\phi$ 。

这段话什么意思呢？就是我们前面算的四元数 $qnb$ 是机体系到计算系的即 $Q^{n'}_b$ ，乘上失准角误差的四元数就得到真实的导航系下的四元数了。也就是：
$Q^{n'}_b \xrightarrow{} Q^n_b$
的过程。而：
$Q^{n'}_b=Q^{n'}_n \circ Q^n_b$
则：
$Q^n_b = (Q^{n'}_n)^* \circ Q^{n'}_b = Q^n_{n'} \circ Q^{n'}_b$
$Q^{n'}_n$ 对应的等效旋转矢量为 $\phi$ ， $Q^n_{n'}$ 对应的等效旋转矢量为 $-\phi$ 。

8、旋转矢量变换为四元数

% 旋转矢量转换为变换四元数

function q = rv2q( rv )

nm2 = rv' *rv;      % 旋转矢量的模方
if nm2 <1.0e-8      % 如果模方很小，则可用泰勒展开前几项求三角函数
    q0 = 1 -nm2 *(1/8 - nm2 /384);
    s = 1/2 - nm2*(1/48 -nm2/3840);
else
    nm = sqrt(nm2);
    q0 = cos(nm /2);
    s = sin( nm /2) /nm;
end
q = [q0; s *rv];

end

当 $nm2 \geq 1.0e^{-8}$ 时，参考：
$Q = cos\frac{\phi}{2} + \frac{\vec{\phi} } {\phi} sin\frac{\phi}{2}$

今天就更新这么多，明天接着注释更新。想想好激动，一开始学习惯导就想看这个程序结果看不懂，一个半月过去了，终于要回头克服这个困难了！选择了，就放手去做！