“psins工具箱140410”版本中基于惯性系双重积分(位置)的初始对准函数aligni0
,在此对部分语句进行说明,使其更加直观易懂。
len = fix(length(imu)/nn)*nn;
之前见到过用工具箱处理自己的数据时会出现下标超限的问题,问题就是没有用这个语句进行数据长度的调整。因为你选的子样数不一定会被你的数据长度整除,所以会出现下标超限的情况。为了避免这种情况,还是fix一下吧。
qib0b = [1; 0; 0; 0];
q b i b 0 q_b^{ib_0} qbib0就是这个了,初始化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 v^{ib_0}, v^{i_0}, p^{ib_0}, p^{i_0}, v^{ib_0}_1, v^{i_0}_2 vib0,vi0,pib0,pi0,v1ib0,v2i0,皆为 [ 0 ; 0 ; 0 ] [0;0;0] [0;0;0]。注意一下, i 0 i_0 i0表示在初始对准起始时刻(即当t=t0=0 时), o x i 0 ox_{i_0} oxi0轴在当地子午面内且平行于赤道平面, o z i 0 oz_{i_0} ozi0轴指向地球自转方向, 三轴构成右手坐标系, 初始对准开始后 i 0 i_0 i0系三轴方向相对惯性空间保持不动;我们要求解的就是 i b 0 ib_0 ib0到 i 0 i_0 i0的姿态阵。显而易见但还是要提一下,两次积分法是对比力积分,积分一次得速度,两次的位置。
[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 , a t t k , a t t k v 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} pkib0,pki0,vki0,vkib0,fki0,fkib0,attk,attkv,皆为: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 ⊗ Δ v / t f^{ib_0}=q^{ib_0}_b \otimes \Delta v/t fib0=qbib0⊗Δv/t,求得b系惯性系的比力;
v i b 0 = v i b 0 + f i b 0 × t v^{ib_0} = v^{ib_0} + f^{ib_0} \times t vib0=vib0+fib0×t,一次积分;
p
i
b
0
=
p
i
b
0
+
(
v
1
i
b
0
+
v
i
b
0
)
×
t
/
2
p^{ib_0} = p^{ib_0} + (v^{ib_0}_1 + v^{ib_0} ) \times t / 2
pib0=pib0+(v1ib0+vib0)×t/2,两次积分,速度取两次子样算法的平均值。
不要关心权重ratio。
该部分是通过加计输出的信息计算得到
i
b
0
ib_0
ib0系的比力、速度位置等信息。
[fi0, vi0, pi0] = i0fvp(kts, lat);
该部分是通过已经历的时间和初始纬度信息计算得到 i 0 i_0 i0系的比力、速度位置等信息。
对于函数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}
fi0的求解可以理解为:
f
i
0
=
−
C
n
i
0
g
n
f^{i_0} =- C_{n}^{i_0} g^n
fi0=−Cni0gn,其中
C
n
i
0
C_{n}^{i_0}
Cni0的求解方法可以参考:严恭敏,秦永元,卫育新,张立川,徐德民,严卫生. 一种适用于SINS动基座初始对准的新算法[J]. 系统工程与电子技术,2009,31(03):634-637. 资源汇总中下载,需要注意的是这是静基座对准,所以
δ
λ
=
0
\delta \lambda=0
δλ=0;
v
i
0
v^{i_0}
vi0 和
p
i
0
p^{i_0}
pi0可以通过对
f
i
0
f^{i_0}
fi0分别进行一次和两次积分求得。
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 C_{n}^{i_0} Cni0求 q n i 0 q^{i_0}_n qni0,出自前面的论文。
qi0ib0 = dv2atti(pi0k(k1,:)', pi0, pib0k(k1,:)', pib0);
qi0ib0 = dv2atti(vi0k(k1,:)', vi0, vib0k(k1,:)', vib0);
以上两语句分别通过位置(两次积分法)和速度(一次积分法)来求解
q
i
b
0
i
0
q^{i_0}_{ib_0}
qib0i0,然后通过
q
b
n
=
q
i
0
n
⊗
q
i
b
0
i
0
⊗
q
b
i
b
0
q^{n}_{b}=q^n_{i_0} \otimes q^{i_0}_{ib_0} \otimes q^{ib_0}_{b}
qbn=qi0n⊗qib0i0⊗qbib0
求得
q
b
n
q^{n}_{b}
qbn。
pi0k
和pib0k
是当前时刻t与初始时刻0的中间时刻所对用的i0系与ib0系的位置,
pi0
和pib0
是当前时刻t所对应的i0系与ib0系的位置,
dv2atti
函数建议参考"psins180508"的求解思路,可参考039解析粗对准matlab备忘