020静态惯性器件捷联惯导仿真(2)

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

021静态惯性器件捷联惯导仿真(3)

---

接着前面的内容进行更新。

---

9、捷联惯导更新算法

% 捷联惯导更新算法
% 参考 (4.1-23)、(4.1-50)、(4.1-57)、(4.1-60)和4.1.1节
% eth.gcc 有害加速度

function [ qnb, vn, pos, eth ] = insupdate( qnb, vn, pos, wm, vm, ts )

nn = size(wm, 1);
nts = nn * ts;
[phim, dvbm] = cnscl(wm, vm); % 补偿后旋转矢量phim，补偿后速度增量dvbm
eth = earth(pos, vn);         % 地球相关参数计算

vn1 = vn + rv2m(-eth.wnin *nts /2) *qmulv(qnb, dvbm) +eth.gcc *nts; % 速度更新
vn = (vn +vn1) /2;
pos = pos + [vn(2) /eth.RMh;  vn(1) /eth.clRNh;  vn(3)] *nts;       % 位置更新
vn = vn1;

qnb = qmul(rv2q(-eth.wnin *nts), qmul(qnb, rv2q(phim)) ); % 姿态更新
qnb = qnormlz(qnb);                                       % 四元数归一化，保证仍为归一化四元数

end

（1）速度更新

参考(4.1-23)和(4.1-50)，与(4.1-50)对应关系：
$$\begin{array}{c}rv2m(-eth.wnin\ast nts/2)\to I-\frac{T}{2}({\omega }_{in(m-1/2)}^n\times )\\ \\ dvbm\to \Delta v_m+\Delta v_{rot(m)}^{b(m-1)}+\Delta v_{scul(m)}^{b(m-1)}\end{array}$$
另外，如果对rv2m(-eth.wnin *nts /2)不理解，可用eye(3)-nts/2*askew(eth.wnin)代替。

（2）位置更新

参考(4.1-57)、(4.1-60)。

（3）姿态更新

参考4.1.1小节。

---

10、圆锥/划船误差补偿

% 圆锥/划船误差补偿
% 参考(2.3-36)、(4.1-36)、(4.1-44)和表2.6-2

function [ phim, dvbm ] = cnscl( wm, vm )

cs = [
    [2, 0, 0, 0, 0] /3
    [9, 27, 0, 0, 0] /20
    [54, 92, 214, 0, 0] /105
    [250, 525, 650, 1375, 0] /504
    [2315, 4558, 7296, 7834, 15797] /4620];   % 2-6子样补偿系数

wmm = sum(wm, 1);         % 将wm的每一行相加，每个采样时间段的角度增量
vmm = sum(vm, 1);         % 将vm的每一行相加，每个采样时间段的速度增量
dphim = zeros(1, 3);
scullm = zeros(1, 3);

n = size(wm, 1);                                         % 每个采样时间段的子样数
if n>1
    csw = cs(n-1, 1:n-1) *wm(1:n-1,:);
    csv = cs(n-1, 1:n-1) *vm(1:n-1,:);
    dphim = cross(csw, wm(n,:));                         % 圆锥补偿量
    scullm = cross(csw, vm(n,:)) +cross(csv, wm(n,:));   % 划船补偿量
end
phim = (wmm +dphim)';                         % 圆锥补偿后的旋转矢量
dvbm = (vmm +0.5*cross(wmm, vmm) +scullm)';   % 旋转和划船补偿后的速度增量

end

如果该函数在理解上比较困难，也就是if之后的内容了。

(1)圆锥补偿

csw = cs(n-1, 1:n-1) *wm(1:n-1,:);
dphim = cross(csw, wm(n,:)); 
phim = (wmm +dphim)';

上面的程序二子样下的圆锥补偿，虽然公式(2.6-36)是在四子样算法下得到的，但我们也可以轻易地将其转化为二子样计算公式：
$$\varphi (T)=\Delta {\theta }_m+\delta \hat{\varphi }(T)=[\Delta {\theta }_m(1)+\Delta {\theta }_m(2)]+k_1\Delta {\theta }_m(1)\times \Delta {\theta }_m(2)$$
整理一下程序与公式的对应关系
$$\begin{array}{c}cs(n-1,1:n-1)\to k_1\\ \\ wm(1:n-1,:)\to \Delta {\theta }_m(1)\\ \\ wm(n,:)\to \Delta {\theta }_m(2)\\ \\ wmm\to \Delta {\theta }_m(1)+\Delta {\theta }_m(2)\end{array}$$

(2)划船补偿

csv = cs(n-1, 1:n-1) *vm(1:n-1,:);
scullm = cross(csw, vm(n,:)) +cross(csv, wm(n,:)); 
dvbm = (vmm +0.5*cross(wmm, vmm) +scullm)';

参考公式(4.1-36)和(4.1-44)。这里不过多介绍，仅罗列一下对应关系：
$$\begin{array}{c}vm(1:n-1,:)\to \Delta v_{m1}\\ \\ vm(n,:)\to \Delta v_{m2}\\ \\ wm(n,:)\to \Delta {\theta }_{m2}\\ \\ 0.5\ast cross(wmm,vmm)\to \frac{1}{2}\Delta {\theta }_m\times \Delta v_m\end{array}$$

其中，0.5*cross(wmm, vmm)表示速度的旋转误差补偿量。
我连下标都跟书上标的一样，圆锥数字用括号，划船数字用下标。

---

11、旋转矢量转换为变换矩阵

% 旋转矢量转换为变换矩阵
% 参考公式 (2.2-23)

function m = rv2m( rv )

nm2 = rv' *rv;                % 旋转矢量的模方 列向量
if nm2 < 1.0e-8               % 如果模方很小，则可用泰勒展开前几项求三角函数
    a = 1 - nm2 *(1/6  - nm2/120);
    b = 0.5 - nm2 *(1/24  - nm2/720);
else
    nm = sqrt(nm2);
    a = sin(nm) /nm;
    b = (1 -cos(nm)) /nm2;
end

VX = askew(rv);
m = eye(3) + a*VX + b*VX^2;   % eye(3) 三维单位矩阵

end

参考公式 (2.2-23)易得上述函数。其中函数函数如下。

---

12、三维向量的反对称阵

% 三维向量的反对称阵
% 参考公式(2.1-3)

function m = askew( v )

m = [0 , -v(3) , v(2);
    v(3) , 0 , -v(1);
    -v(2) , v(1) , 0;
    ];

end

---

13、四元数归一化

% 四元数归一化
% 参考公式(2.4-19)与(2.4-20)之间文字部分
function qnb = qnormlz( qnb )

nm = qnb' *qnb;
if nm < 1e-6
    qnb =[1; 0; 0; 0];   % 表示姿态的四元数，其模值应为1
else
    qnb = qnb /sqrt(nm);
end

end

---

14、四元数转换为姿态角

% 四元数转换为姿态角

function att = q2att( qnb )

att = m2att(q2mat(qnb));

end

函数没m2att与q2mat如下。

---

15、姿态阵转换为姿态角

% 姿态阵转换为姿态角
% 参考公式(B-7)
function att = m2att( Cnb )

if abs(Cnb(3,2)) <= 0.999999
    att = [ asin(Cnb(3,2)); -atan2(Cnb(3,1),Cnb(3,3)); -atan2(Cnb(1,2),Cnb(2,2))];
else
    att = [ asin(Cnb(3,2)); atan2(Cnb(1,3),Cnb(1,1)); 0];

end

---

16、四元数转换为姿态阵

% 四元数转换为姿态阵
% 公式详见 （2.4-23） P23

function Cnb = q2mat( qnb )

q11 = qnb(1)*qnb(1); q12 = qnb(1)*qnb(2); q13 = qnb(1)*qnb(3); q14 = qnb(1)*qnb(4); 
q22 = qnb(2)*qnb(2); q23 = qnb(2)*qnb(3); q24 = qnb(2)*qnb(4); 
q33 = qnb(3)*qnb(3); q34 = qnb(3)*qnb(4); 
q44 = qnb(4)*qnb(4); 

Cnb = [
    q11+q22-q33-q44, 2*(q23-q14),     2*(q24+q13);
    2*(q23+q14),     q11-q22+q33-q44, 2*(q34-q12);
    2*(q24-q13),     2*(q34+q12),     q11-q22-q33+q44
];
end

---

17、辅助函数—作图

% 辅助函数---作图
function mysubplot( mnp, x, y, xstr, ystr )

if mod(mnp, 10) == 1
    figure;
end                        % 如果是第一幅小图，则新建一个figure
subplot(mnp);
plot(x, y);
grid on;

if nargin == 4
    ystr = xstr;
    xstr = 't/s';
end
xlabel(xstr);
ylabel(ystr);

end

---

18、辅助函数—求位置增量

% 辅助函数---求位置增量
function dpos = deltapos( pos )

dpos = [[(pos(:,1) - pos(1,1)), (pos(:,2) - pos(1,2)) .*cos(pos(:,1))] *6378137, pos(:,3) - pos(1,3)];

end

---

主函数在下一篇再写，因为打算画一个流程图，把思绪理清楚！GOGOGO！