[发明专利]一种量测干扰下用于飞行器姿态估计的鲁棒递推滤波方法

权利要求书

1.一种量测干扰下用于飞行器姿态估计的鲁棒递推滤波方法，其特征在于：

(1)采集飞行器运动过程中陀螺与星敏感器的输出数据；

(2)建立量测干扰下基于姿态四元数的飞行器非线性状态空间模型；

(2.1)建立飞行器姿态估计系统的状态方程；

将姿态四元数q_k和陀螺漂移β_k组成维数为n的状态变量xk=qkTβkTT,]]>建立基于姿态四元数的飞行器状态方程为：

xk+1=f(xk,ω~k)+Σi=1sAikηikxk+wk]]>

f(xk,ω~k)=(cos(||ω~k-βk||Δt2)I4×4+sin(||ω~k-βk||Δt2)Ω(ω~k-βk)||ω~k-βk||)qkβk;]]>q_k为k时刻的姿态四元数；β_k为k时刻的陀螺漂移值；为k时刻的陀螺输出测量值；Δt为陀螺的采样时间；||·||为求范数符号；a＝[a₁ a₂ a₃]^T为三维矢量，Ω(a)=-[a×]a-aT0,[a×]=0-a3a2a30-a1-a2a10;wk=04×1ηu]]>是加性噪声，方差Qk=E(wkwkT)=04×404×303×4Δtσu2I3×3,]]>η_u为陀螺漂移测量噪声，η_u的方差为s＝3：η_ik是均值为零，方差为1的高斯白噪声；A_ik为恰当维数的矩阵：

Aik=-Δtσv2Aik104×303×403×3]]>

A1k1=000100100-100-1000;A2k1=00-10000110000-100;A3k1=0100-1000000100-10;]]>σ_v为陀螺测量噪声；

(2.2)建立量测干扰下飞行器姿态估计系统的量测方程；

量测干扰下星敏感器的测量模型：

为k时刻星敏感器量测值；为星敏感器的参考矢量；i为星敏感器观测到恒星的个数；为零均值的高斯白噪声，的方差为为未知的量测干扰向量；A(q_k)为四元数q_k的姿态描述矩阵，qk=ρkTq4T,]]>A(q_k)为：

A(qk)=(q42-ρkTρk)I3×3+2ρkρkT-2q4[ρk×]]]>

将未知的量测干扰矩阵转化为：

Mi=00σiy0σiz0σix0000σiz0σix0σiy00;]]>Δ_i＝diag([Δ_ix Δ_ix Δ_iy Δ_iy Δ_iz Δ_iz])；j＝x，y，z；Ei=00010-10-1001010-1000T;]]>σ_ij，j＝x，y，z为已知的正常数；

采用3颗星的测量数据，即i＝3，飞行器姿态估计系统的量测方程被建立为：

z_k＝h(x_k)+MΔg(x_k)+v_k

zk=zk1zk2zk3]]>为k时刻系统的量测值，维数为m；h(xk)=A(qk)r→1A(qk)r→2A(qk)r→3]]>为量测非线性函数；g(x_k)＝Eh(x_k)；M=M1M2M3;E=E1E2E3;Δ=Δ1Δ2Δ3,]]>ΔΔ^T≤I_18×18；v_k是零均值高斯白噪声，v_k的方差为

(3)进行时间更新，求得一步状态预测和预测方差的上界；

k时刻的状态估计值为方差上界为Σ_k，求得一步状态预测值为：

x^k+1|k=f(x^k,ω~k)]]>

一步状态预测误差为：

x~k+1|k=xk+1-x^k+1|k=f(xk,ω~k)-f(x^k,ω~k)+Σi=1sAikηikxk+wk]]>

将在处进行泰勒展开有：

f(xk,ω~k)=f(x^k,ω~k)+Fkx~k+AkβkLkx~k]]>

A_k为已知的比例矩阵；β_k为未知矩阵，满足L_k为已知的调节矩阵，通常设为

一步状态预测误差变形为：

x~k+1|k=(Fk+AkβkLk)x~k+Σi=1sAikηikxk+wk]]>

一步状态预测方差为：

Pk+1|k=E[(Fk+AkβkLk)x~kx~kT(Fk+AkβkLk)T]+Σi=1sAikE(xkxkT)AikT+Qk=(Fk+AkβkLk)Σk(Fk+AkβkLk)T+Σi=1sAikE(xkxkT)AikT+Qk≤Fk(Σk-1-λLkTLk)-1FkT+λ-1AkAkT+Σi=1sAik[(1+ϵ)Σk+(1+ϵ-1)x^kx^kT]AikT+Qk]]>

ε和λ都为已知的正数，λ满足

λ-1In×n-LkΣkLkT>0;]]>

一步状态预测方差的上界为：

Σk+1|k=Fk(Σk-1-λLkTLk)-1FkT+λ-1AkAkT+Σi=1sAik[(1+ϵ)Σk+(1+ϵ-1)x^kx^kT]AikT+Qk]]>

(4)进行鲁棒递推滤波量测更新，求得最优的滤波增益，进而求出k+1时刻的状态估计值和方差的上界，将k+1时刻的状态估计中四元数部分进行强制的归一化约束；

一步量测预测值为：

z^k+1=h(x^k+1|k)]]>

一步量测预测误差为：

z~k+1=zk+1-h(x^k+1|k)=h(xk+1)-h(x^k+1|k)+MΔg(xk+1)+vk+1]]>

将h(x_k+1)在处进行泰勒展开有：

h(xk+1)=h(x^k+1|k)+Hk+1x~k+1|k+Ck+1αk+1Lk+1x~k+1|k]]>

式中，C_k+1为已知的比例矩阵；α_k+1为未知矩阵，满足

一步量测预测误差能够变形为：

z~k+1=(Hk+1+Ck+1αk+1Lk+1)x~k+1|k+MΔg(xk+1)+vk+1]]>

k+1时刻的状态估计为：

x^k+1=x^k+1|k+Kk+1z~k+1]]>

K_k+1为待求的滤波增益；

则k+1时刻的估计误差为：

x~k+1=xk+1-x^k+1=(In×n-Kk+1Hk+1-Kk+1Ck+1αk+1Lk+1)x~k+1|k-Kk+1[MΔg(xk)+vk+1]]]>

k+1时刻的估计误差协方差矩阵为：

Pk+1=E[x~k+1x~k+1T]=(In×n-Kk+1Hk+1-Kk+1Ck+1αk+1Lk+1)Σk+1|k(In×n-Kk+1Hk+1-Kk+1Ck+1αk+1Lk+1)T+E[Kk+1MΔg(xk+1)gT(xk+1)ΔTMTKk+1T]+Kk+1Rk+1Kk+1T-E[(In×n-Kk+1Hk+1-Kk+1Ck+1αk+1Lk+1)x~k+1|kgT(xk+1)ΔTMTKk+1T]-E[Kk+1MΔg(xk+1)x~k+1|kT(In×n-Kk+1Hk+1-Kk+1Ck+1αk+1Lk+1)T]]]>

-(In×n-Kk+1Hk+1-Kk+1Ck+1αk+1Lk+1)x~k+1|kgT(xk+1)ΔTMTKk+1T-Kk+1MΔg(xk+1)x~k+1|kT(In×n-Kk+1Hk+1-Kk+1Ck+1αk+1Lk+1)T≤ϵ1(In×n-Kk+1Hk+1-Kk+1Ck+1αk+1Lk+1)x~k+1|kx~k+1|kT(In×n-Kk+1Hk+1-Kk+1Ck+1αk+1Lk+1)T+ϵ1-1Kk+1MΔg(xk+1)gT(xk+1)ΔTMTKk+1T]]>

估计误差协方差矩阵变形为：

Pk+1≤(1+ϵ1)(In×n-Kk+1Hk+1-Kk+1Ck+1αk+1Lk+1)Σk+1|k(In×n-Kk+1Hk+1-Kk+1Ck+1αk+1Lk+1)T+(1+ϵ1-1)Kk+1ME[Δg(xk+1)gT(xk+1)ΔT]MTKk+1T+Kk+1Rk+1Kk+1T≤(1+ϵ1)[(In×n-Kk+1Hk+1)(Σk+1|k-1-μLk+1TLk+1)-1(In×n-Kk+1Hk+1)T+μ-1Kk+1Ck+1Ck+1TKk+1T]+(1+ϵ1-1)Kk+1MMTKk+1T+Kk+1Rk+1Kk+1T]]>

μ为已知的正数，满足μ-1In×n-Lk+1Pk+1|kLk+1T>0;]]>

k+1时刻的估计误差的上界为：

Σk+1=(1+ϵ1)[(In×n-Kk+1Hk+1)(Σk+1|k-1-μLk+1TLk+1)-1(In×n-Kk+1Hk+1)T+μ-1Kk+1Ck+1Ck+1TKk+1T]+(1+ϵ1-1)Kk+1MMTKk+1T+Kk+1Rk+1Kk+1T]]>

令求得最优滤波增益为：

Kk+1=(1+ϵ1)(Σk+1|k-1-μLk+1TLk+1)-1Hk+1T×{(1+ϵ1)Hk+1(Σk+1|k-1-μLk+1TLk+1)-1Hk+1T+μ-1Ck+1Ck+1T+(1+ϵ1-1)MMT+Rk+1}-1]]>

求出k+1时刻的状态估计值和方差的上界Σ_k+1；

‖q_k‖₂＝1，再将k+1时刻的状态估计值中的四元数部分进行归一化约束；

(5)姿态估计系统的运行时间为M，若k＝M，则输出姿态四元数及陀螺漂移的结果，完成姿态估计；若k＜M，表示滤波过程未完成，则重复步骤(3)至步骤(4)，直至姿态估计系统运行结束。