043基于惯性系一次(速度)及双重积分(位置)的初始对准matlab笔记

“psins工具箱140410”版本中基于惯性系双重积分(位置)的初始对准函数aligni0，在此对部分语句进行说明，使其更加直观易懂。

---

len = fix(length(imu)/nn)*nn;

之前见到过用工具箱处理自己的数据时会出现下标超限的问题，问题就是没有用这个语句进行数据长度的调整。因为你选的子样数不一定会被你的数据长度整除，所以会出现下标超限的情况。为了避免这种情况，还是fix一下吧。

---

qib0b = [1; 0; 0; 0];

$q_b^{i{b}_0}$就是这个了，初始化b系到b在t0时刻凝固生成的惯性系之间的旋转四元数，后面通过陀螺测得的数据对其进行更新。

---

[vib0, vi0, pib0, pi0, vib0_1, vi0_1] = setvals(zeros(3,1));

依旧是初始化：$v^{i{b}_0},v^{i_0},p^{i{b}_0},p^{i_0},v_1^{i{b}_0},v_2^{i_0}$，皆为$[0;0;0]$。注意一下，$i_0$表示在初始对准起始时刻(即当t=t0=0 时),$o{x}_{i_0}$轴在当地子午面内且平行于赤道平面,$o{z}_{i_0}$轴指向地球自转方向, 三轴构成右手坐标系, 初始对准开始后$i_0$系三轴方向相对惯性空间保持不动;我们要求解的就是$i{b}_0$到$i_0$的姿态阵。显而易见但还是要提一下，两次积分法是对比力积分，积分一次得速度，两次的位置。

---

[pib0k, pi0k, vi0k, vib0k, fi0k, fib0k, attk, attkv] = prealloc(len/nn, 3);

还是初始化，$p_k^{i{b}_0},p_k^{i_0},v_k^{i_0},v_k^{i{b}_0},f_k^{i_0},f_k^{i{b}_0},at{t}_k,at{t}_{kv}$，皆为：2500*3的零矩阵。

另外对于prealloc还是简单说一下。
varargout作为一个cell包含输出的八个元素，nargout用于检测输出个数，在此等于8。varargin作为除了第一个元素之外的所有元素的集合cell，在此只有一个元素为3，所以以上八个元素都被赋值为2500*3的零矩阵。

---

k0 = fix(5/ts);

前五秒不进行对准，但是后面的对准中也用到前五秒的数据。

---

kts = (k+nn-1)*ts;

从开始到当前时刻经历的时间长度。

---

fib0 = qmulv(qib0b, dvbm)/nts;
vib0 = vib0 + fib0*nts;
pib0 = ratio*pib0 + (vib0_1+vib0)*nts/2;  
vib0_1 = vib0;

即：$f^{i{b}_0}=q_b^{i{b}_0}\otimes \Delta v/t$，求得b系惯性系的比力;

$v^{i{b}_0}=v^{i{b}_0}+f^{i{b}_0}\times t$，一次积分;

$p^{i{b}_0}=p^{i{b}_0}+(v_1^{i{b}_0}+v^{i{b}_0})\times t/2$，两次积分，速度取两次子样算法的平均值。
不要关心权重ratio。
该部分是通过加计输出的信息计算得到$i{b}_0$系的比力、速度位置等信息。

---

[fi0, vi0, pi0] = i0fvp(kts, lat);

该部分是通过已经历的时间和初始纬度信息计算得到$i_0$系的比力、速度位置等信息。

对于函数i0fvp

fi0 = fcL*[cwt; swt; tL];
vi0 = fcL/glv.wie*[swt; 1-cwt; tL*wt];
pi0 = fcL/glv.wie^2*[1-cwt; wt-swt; tL*wt^2/2];

$f^{i_0}$的求解可以理解为：$f^{i_0}=-C_n^{i_0}{g}^n$，其中$C_n^{i_0}$的求解方法可以参考:严恭敏,秦永元,卫育新,张立川,徐德民,严卫生. 一种适用于SINS动基座初始对准的新算法[J]. 系统工程与电子技术,2009,31(03):634-637. 资源汇总中下载，需要注意的是这是静基座对准，所以$\delta \lambda =0$;
$v^{i_0}$ 和 $p^{i_0}$可以通过对$f^{i_0}$分别进行一次和两次积分求得。

---

Cni0 = [-swiet,         cwiet,          0; 
        -eth.sl*cwiet,  -eth.sl*swiet,  eth.cl; 
        eth.cl*cwiet,   eth.cl*swiet,   eth.sl];
qni0 = m2qua(Cni0);

通过$C_n^{i_0}$求$q_n^{i_0}$，出自前面的论文。

---

qi0ib0 = dv2atti(pi0k(k1,:)', pi0, pib0k(k1,:)', pib0);

qi0ib0 = dv2atti(vi0k(k1,:)', vi0, vib0k(k1,:)', vib0);

以上两语句分别通过位置(两次积分法)和速度(一次积分法)来求解$q_{i{b}_0}^{i_0}$，然后通过
$$q_b^n=q_{i_0}^n\otimes q_{i{b}_0}^{i_0}\otimes q_b^{i{b}_0}$$
求得 $q_b^n$。

pi0k和pib0k是当前时刻t与初始时刻0的中间时刻所对用的i0系与ib0系的位置，
pi0和pib0是当前时刻t所对应的i0系与ib0系的位置，
dv2atti函数建议参考"psins180508"的求解思路，可参考039解析粗对准matlab备忘