“psins工具箱140410”版本中基於慣性系雙重積分(位置)的初始對準函式aligni0，在此對部分語句進行說明，使其更加直觀易懂。

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

之前見到過用工具箱處理自己的資料時會出現下標超限的問題，問題就是沒有用這個語句進行資料長度的調整。因為你選的子樣數不一定會被你的資料長度整除，所以會出現下標超限的情況。為了避免這種情況，還是fix一下吧。

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

$q_b^{ib_0}$就是這個了，初始化b系到b在t0時刻凝固生成的慣性系之間的旋轉四元數，後面通過陀螺測得的資料對其進行更新。

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

依舊是初始化：$v^{ib_0}, v^{i_0}, p^{ib_0}, p^{i_0}, v^{ib_0}_1, v^{i_0}_2$，皆為$[0;0;0]$。注意一下，$i_0$表示在初始對準起始時刻(即當t=t0=0 時),$ox_{i_0} $軸在當地子午面內且平行於赤道平面,$oz_{i_0}$軸指向地球自轉方向, 三軸構成右手座標系, 初始對準開始後$

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

還是初始化，$p^{ib_0}_k, p^{i_0}_k, v^{i_0}_k, v^{ib_0}_k, f^{i_0}_k, f^{ib_0}_k, att_k, att_{kv}$

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^{ib_0}=q^{ib_0}_b \otimes \Delta v/t$，求得b系慣性系的比力;

$v^{ib_0} = v^{ib_0} + f^{ib_0} \times t$，一次積分;

$p^{ib_0} = p^{ib_0} + (v^{ib_0}_1 + v^{ib_0} ) \times t / 2$，兩次積分，速度取兩次子樣演算法的平均值。 不要關心權重ratio。 該部分是通過加計輸出的資訊計算得到$ib_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^{i_0}_n$，出自前面的論文。

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

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

以上兩語句分別通過位置(兩次積分法)和速度(一次積分法)來求解$q^{i_0}_{ib_0}$，然後通過 $q^{n}_{b}=q^n_{i_0} \otimes q^{i_0}_{ib_0} \otimes q^{ib_0}_{b}$ 求得 $q^{n}_{b}$。

pi0k和pib0k是當前時刻t與初始時刻0的中間時刻所對用的i0系與ib0系的位置， pi0和pib0是當前時刻t所對應的i0系與ib0系的位置， dv2atti函式建議參考"psins180508"的求解思路，可參考039解析粗對準matlab備忘