捷联惯导系统学习2.6(圆锥误差补偿多子样算法)

若圆锥运动的四元数更新方程为：
Q(tm)=Q(Tm−1)。Q(T)Q(t_m)=Q(T_{m-1})。Q(T)Q(tm)=Q(Tm−1)。Q(T)
（。。。四元数乘法）
（ Q(T)Q(T)Q(T)四元数周期内变化量）
得到：（Ω\OmegaΩ震动频率;θ=∣ϕ∣\theta=|\phi|θ=∣ϕ∣）
Q(T)=Q∗(tm−1)Q(Tm)=MQ∗(tm−1)Q(tm)Q(T)=Q^*(t_{m-1})Q(T_{m})=M_{Q^*(t_{m-1})}Q(t_m)Q(T)=Q∗(tm−1)Q(Tm)=MQ∗(tm−1)Q(tm)
=[1−2(sin(θ2)sinΩT2)2−sinθsinΩT2sinΩ(tm−T2)sinθsinΩT2cosΩ(tm−T2)−sin2θ2sinΩT](推导略)=[cosθ2u∗sin(θ2)]=\left[\begin{matrix} 1-2(sin(\frac{\theta}{2})sin\frac{\Omega T}{2})^2 \\ -sin\theta sin\frac{\Omega T}{2}sin\Omega(t_m-\frac{T}{2})\\ sin\theta sin\frac{\Omega T}{2}cos\Omega(t_m-\frac{T}{2})\\ -sin^2\frac{\theta}{2}sin\Omega T \end{matrix}\right](推导略)=\left[\begin{matrix} cos\frac{\theta}{2}\\ u*sin(\frac{\theta}{2})\\ \end{matrix}\right]=⎣⎢⎢⎡1−2(sin(2θ)sin2ΩT)2−sinθsin2ΩTsinΩ(tm−2T)sinθsin2ΩTcosΩ(tm−2T)−sin22θsinΩT⎦⎥⎥⎤(推导略)=[cos2θu∗sin(2θ)]
若：在[tm−1,tm][t_{m-1},tm][tm−1,tm]时间内的等效旋转矢量为ϕ(T)\phi(T)ϕ(T);u=ϕ(T)θu=\frac{\phi(T)}{\theta}u=θϕ(T)
得到ϕ(T)θ(t)∗sin(θ(t)2)=[−sinθsinΩT2sinΩ(tm−T2)sinθsinΩT2cosΩ(tm−T2)−sin2θ2sinΩT]\frac{\phi(T)}{\theta(t)}*sin(\frac{\theta(t)}{2})=\left[\begin{matrix} -sin\theta sin\frac{\Omega T}{2}sin\Omega(t_m-\frac{T}{2})\\ sin\theta sin\frac{\Omega T}{2}cos\Omega(t_m-\frac{T}{2})\\ -sin^2\frac{\theta}{2}sin\Omega T \end{matrix}\right]θ(t)ϕ(T)∗sin(2θ(t))=⎣⎡−sinθsin2ΩTsinΩ(tm−2T)sinθsin2ΩTcosΩ(tm−2T)−sin22θsinΩT⎦⎤
左右两边取模值，得到
sinΩT2=sin2θsin2ΩT2+sin4θ2sin2ΩTsin\frac{\Omega T}{2}=\sqrt{sin^2\theta sin^2 \frac{\Omega T}{2}+sin^4\frac{\theta}{2}sin^2 {\Omega T}}sin2ΩT=sin2θsin22ΩT+sin42θsin2ΩT
当θ和ΩT\theta和\Omega Tθ和ΩT均为小量时：
sinΩT2≈sin2θsin2ΩT2≈θΩT2sin\frac{\Omega T}{2}\approx\sqrt{sin^2\theta sin^2 \frac{\Omega T}{2}}\approx\frac{\theta \Omega T}{2}sin2ΩT≈sin2θsin22ΩT≈2θΩT
从而进一步的到： θ(t)=θΩT\theta(t)=\theta\Omega Tθ(t)=θΩT

多子样补偿算法
当使用角增量代替旋转矢量进行姿态更新时会在z轴产生误差，这个误差会随时间不断积累，常使用多子样补偿算法，弥补这一算法(证略)
在[tm−1,tm][t_{m-1},t_{m}][tm−1,tm]内采样N次，h=TNh=\frac{T}{N}h=NT为采样间隔
Δθmi=∫tm−1+(i−1)htm−1+ihw(t)dt\Delta \theta_{mi}=\int_{t_{m-1}+(i-1)h}^{t_{m-1}+ih}w(t)dtΔθmi=∫tm−1+(i−1)htm−1+ihw(t)dt
=[−2sinθsinΩT2sinΩ(tm−1+ih−h2)sinθsinΩT2cosΩ(tm−1+ih−h2)−sin2θ2sinΩT]=\left[\begin{matrix} -2sin\theta sin\frac{\Omega T}{2}sin\Omega(t_{m-1}+ih-\frac{h}{2})\\ sin\theta sin\frac{\Omega T}{2}cos\Omega(t_{m-1}+ih-\frac{h}{2})\\ -sin^2\frac{\theta}{2}sin\Omega T \end{matrix}\right]=⎣⎡−2sinθsin2ΩTsinΩ(tm−1+ih−2h)sinθsin2ΩTcosΩ(tm−1+ih−2h)−sin22θsinΩT⎦⎤
记λ=ΩT\lambda=\Omega Tλ=ΩT得到在[tm−1,tm][t_{m-1},t_{m}][tm−1,tm]内的总角增量为：
Δθm=∫tm−1tmw(t)dt=∑i=1NΔθmi\Delta\theta_m=\int_{t_{m-1}}^{t_m}w(t)dt=\sum_{i=1}^N\Delta\theta_{mi}Δθm=∫tm−1tmw(t)dt=i=1∑NΔθmi
求解不同子样的角增量之间的叉积（θ和λ\theta和\lambdaθ和λ均为小量）
Δθmi×Δθmj=[8λsin2θ2sinθsinλ2sin(i−j)λ2sinΩ(tm−1−i+j−12)h−8λsin2θ2sinθsinλ2sin(i−j)λ2cosΩ(tm−1−i+j−12)h−4sin2θ2sin2λ2sin(i−j)λ]\Delta \theta_{mi}\times\Delta \theta_{mj}=\left[\begin{matrix} 8\lambda sin^2\frac{\theta}{2}sin\theta sin\frac{\lambda}{2}sin\frac{(i-j)\lambda}{2}sin\Omega(t_{m-1}-\frac{i+j-1}{2})h\\ -8\lambda sin^2\frac{\theta}{2}sin\theta sin\frac{\lambda}{2}sin\frac{(i-j)\lambda}{2}cos\Omega(t_{m-1}-\frac{i+j-1}{2})h\\ -4sin^2\frac{\theta}{2}sin^2\frac{\lambda}{2}sin(i-j)\lambda\\ \end{matrix}\right]Δθmi×Δθmj=⎣⎡8λsin22θsinθsin2λsin2(i−j)λsinΩ(tm−1−2i+j−1)h−8λsin22θsinθsin2λsin2(i−j)λcosΩ(tm−1−2i+j−1)h−4sin22θsin22λsin(i−j)λ⎦⎤
θ和λ\theta和\lambdaθ和λ均为小量
≈[(i−j)(θλ)32sinΩ(tm−1−i+j−12)h−(i−j)(θλ)32cosΩ(tm−1−i+j−12)h−4sin2θ2sin2λ2sin(i−j)λ]\approx \left[\begin{matrix} \frac{(i-j)(\theta\lambda)^3}{2}sin\Omega(t_{m-1}-\frac{i+j-1}{2})h\\ -\frac{(i-j)(\theta\lambda)^3}{2}cos\Omega(t_{m-1}-\frac{i+j-1}{2})h\\ -4sin^2\frac{\theta}{2}sin^2\frac{\lambda}{2}sin(i-j)\lambda\\ \end{matrix}\right]≈⎣⎢⎡2(i−j)(θλ)3sinΩ(tm−1−2i+j−1)h−2(i−j)(θλ)3cosΩ(tm−1−2i+j−1)h−4sin22θsin22λsin(i−j)λ⎦⎥⎤
定义圆锥误差补偿系数
δθ∧(T)=∑j=2N∑i=1j−1kijΔθmi×Δθmj=∑i=1N−1kN−1∗Δθmi×Δθmj\delta\theta^\wedge(T)=\sum_{j=2}^N\sum_{i=1}^{j-1}k_{ij}\Delta\theta_{mi}\times\Delta\theta_{mj}=\sum_{i=1}^{N-1}k^*_{N-1}\Delta\theta_{mi}\times\Delta\theta_{mj}δθ∧(T)=j=2∑Ni=1∑j−1kijΔθmi×Δθmj=i=1∑N−1kN−1∗Δθmi×Δθmj

kN−1=k1(N−1)∗k_{N-1}=k^*_{1(N-1)}kN−1=k1(N−1)∗
kN−2=k1(N−1)∗+k2(N)∗k_{N-2}=k^*_{1(N-1)}+k^*_{2(N)}kN−2=k1(N−1)∗+k2(N)∗
…
k2=k13∗+k24∗+k35∗+...+k(N−2)(N)∗k_{2}=k^*_{13}+k^*_{24}+k^*_{35}+...+k^*_{(N-2)(N)}k2=k13∗+k24∗+k35∗+...+k(N−2)(N)∗
k1=k12∗+k23∗+k34∗+...+k(N−1)(N)∗k_{1}=k^*_{12}+k^*_{23}+k^*_{34}+...+k^*_{(N-1)(N)}k1=k12∗+k23∗+k34∗+...+k(N−1)(N)∗
因为x,y轴上的误差为(θλ)3(\theta\lambda)^3(θλ)3量级，不会起时姿态累计漂移，所以只用考虑z轴上的分量,将z轴分量，用泰勒级数展开为：
δθ∧(T)z=(ϕ(T)−Δθm)z=2sin2θ2(ΩT−sinΩT)\delta\theta^\wedge(T)_z=(\phi(T)-\Delta\theta_m)_z=2sin^2\frac{\theta}{2}(\Omega T-sin\Omega T)δθ∧(T)z=(ϕ(T)−Δθm)z=2sin22θ(ΩT−sinΩT)
=4sin2θ2∑i=1∞(−1)i+1ciλ2i+1(ci=N2i+12(2i+1)!)=4sin^2\frac{\theta}{2}\sum_{i=1}^{\infty}(-1)^{i+1}c_i\lambda^{2i+1}(c_i=\frac{N^{2i+1}}{2(2i+1)!})=4sin22θi=1∑∞(−1)i+1ciλ2i+1(ci=2(2i+1)!N2i+1)
δθ∧(T)z=∑j=2N∑i=1j−1kijΔθmi×Δθmj=sin2θ∑i=1∞(−1)i+1∑j=1N−1Aijkjλ2i+1\delta\theta^\wedge(T)_z=\sum_{j=2}^N\sum_{i=1}^{j-1}k_{ij}\Delta\theta_{mi}\times\Delta\theta_{mj}=sin^2\theta\sum_{i=1}^{\infty}(-1)^{i+1}\sum_{j=1}^{N-1}A_{ij}k_j\lambda^{2i+1}δθ∧(T)z=j=2∑Ni=1∑j−1kijΔθmi×Δθmj=sin2θi=1∑∞(−1)i+1j=1∑N−1Aijkjλ2i+1
使λ2i+1\lambda^{2i+1}λ2i+1对应系数相等，建立线性方程求解KKK可以得到：
AK=CAK=CAK=C
其中:(i,j=1,2,3,…,N-1)
A=(Aij)(N−1)(N−1)=(j+1)2i+1+(j+1)2i−1−(2j)2i+1(2i+1)!A=(A_{ij})_{(N-1)(N-1)}=\frac{(j+1)^{2i+1}+(j+1)^{2i-1}-(2j)^{2i+1}}{(2i+1)!}A=(Aij)(N−1)(N−1)=(2i+1)!(j+1)2i+1+(j+1)2i−1−(2j)2i+1
K=(kj)(N−1)×1K=(k_j)_{(N-1)\times1}K=(kj)(N−1)×1
C=(ci)(N−1)×1=N2i+12(2i+1)!C=(c_i)_{(N-1)\times1}=\frac{N^{2i+1}}{2(2i+1)!}C=(ci)(N−1)×1=2(2i+1)!N2i+1
误差漂移系数为:(只要θ2(ΩT)2N+1T<1。h\frac{\theta^2(\Omega T)^{2N+1}}{T}<\frac{1^。}{h}Tθ2(ΩT)2N+1<h1。就能满足多数惯性导航系统要求)
ξn=ρnθ2(ΩT)2N+1T(N≥1)\xi_n=\rho_n\frac{\theta^2(\Omega T)^{2N+1}}{T}(N\geq1)ξn=ρnTθ2(ΩT)2N+1(N≥1)

圆锥误差补偿系数和漂移误差分数解

N	k1	k2	k3	k4	k5	ρn
1	-					1/12
2	2/3					1/960
3	27/20	9/12				1/204120
4	214/105	92/105	54/105			1/82575360
5	1375/504	650/504	525/504	250/504		1/82575360
6	15797/4620	7834/4620	7296/4620	4558/4620	2315/4620	1/82575360

EX:四子样子求解等效旋转矢量
ϕ(T)=Δθm+δθ∧(T)=∑i=14Δθmi+∑i=14−1k4−1∗Δθmi×Δθmj\phi(T)=\Delta\theta_m+\delta\theta^\wedge(T)=\sum_{i=1}^4\Delta\theta_{mi}+\sum_{i=1}^{4-1}k^*_{4-1}\Delta\theta_{mi}\times\Delta\theta_{mj}ϕ(T)=Δθm+δθ∧(T)=i=1∑4Δθmi+i=1∑4−1k4−1∗Δθmi×Δθmj
(Δθm1+Δθm2+Δθm3+Δθm4)+(k1Δθm1+k2Δθm2+k3Δθm3)×Δθm4(\Delta\theta_{m1}+\Delta\theta_{m2}+\Delta\theta_{m3}+\Delta\theta_{m4})+(k_1\Delta\theta_{m1}+k_2\Delta\theta_{m2}+k_3\Delta\theta_{m3})\times\Delta\theta_{m4}(Δθm1+Δθm2+Δθm3+Δθm4)+(k1Δθm1+k2Δθm2+k3Δθm3)×Δθm4

捷联惯导系统学习2.6(圆锥误差补偿多子样算法)相关推荐

捷联惯导系统学习2.6(圆锥运动的4种表达方式)
机械陀螺仪的基本特性 (章动):当陀螺仪收到瞬时冲击力矩后,自转轴在原位置附近做微小的圆锥运动,期转子轴的大小方向不变. 圆锥运动(震动环境中产生 ):刚体在受到环境影响或者本身具有角速度时,在两个正 ...
捷联惯导系统学习2.5（等效旋转矢量微分方程的泰勒级数解）
在高精度的捷联惯导系统中,陀螺仪姿态的解算往往是通过采集一定时间内的角增量信息, 计算角增量信息计算出等效旋转矢量,在通过等效旋转矢量递推余弦阵或者四元数,完成姿态更新. 等效旋转矢量微分方程的泰勒级 ...
捷联惯导系统学习4.1(惯导数值更新算法)
1 常用坐标系的定义 (1)地心惯性坐标系(i 系,inertial frame) 用oixiyizio_ix_iy_iz_ioixiyizi表示,原点以地球为中心, 原点oio_ioi在地 ...
捷联惯导系统学习7.5(低成本组合导航系统模型)
低成本组合导航系统模型低精度MEMS惯性/卫星/地磁组合导航系统中,选择惯导系统的姿态失准角ϕ\phiϕ.速度误差δvn\delta v^nδvn.定位误差δpn\delta p^nδpn.陀螺仪相 ...
捷联惯导系统学习3.2(地球的正常重力场)
圆球模型下的地球重力如图,重力为引力与离心力作用的共同结果,其中引力:F=GMr2=ur2(G引力常数,M为地球质量,r为质点到地心距离)F=\frac{GM}{r^2}=\frac{u}{r^2 ...
捷联惯导系统学习6.1(一些卡尔曼滤波处理技术 )
噪声相关条件下的Kalman滤波理想状态下的kalman需要系统噪声与测量噪声之间部不相关,如果测量噪声与系统噪声相关需要进行处理噪声相关条件下的系统状态方程 Xk:n维状态向量X_k:n维状态向 ...
捷联惯导系统学习6.6(Sage-Husa自适应滤波 )
原理作用只有准确的获得系统的结构参数和噪声统计特性参数,才能获得最优值的状态估计,实际中往往是不够准确的可以使用量测输出(输出隐含了系统模型的某些信息)对系统系统模型进行重新估计. 量测噪声方差阵 ...
捷联惯导系统学习2.2（等效旋转矢量）
二等效旋转矢量: 1 一些重要的三维矢量运算关系(证明请自己找) $ u为单位矢量 ;u'是u的一阶导数$ (1):V1×(V2×V3)=(V1∗V3)V2−(V1∗V2)V3(1):V_1\tim ...
捷联惯导系统学习2.5（等效旋转矢量微分方程）
已知三维旋转矢量关系如下:(证明略) 参数说明: ViV_iVi表示三维空间矢量 v=∣V∣=VVTv=|V|=\sqrt{VV^T}v=∣V∣=VVT表示矢量模值 uuu为与V同方向的单位矢量即 ...

捷联惯导系统学习2.6(圆锥误差补偿多子样算法)

捷联惯导系统学习2.6(圆锥误差补偿多子样算法)相关推荐

最新文章

热门文章