[发明专利]一种基于迭代容积卡尔曼滤波姿态估计方法

权利要求书

1.一种基于迭代容积卡尔曼滤波姿态估计方法，其特征在于，包括以下几个步骤：

步骤一：采集飞行器运动过程中陀螺与星敏感器的输出数据作为量测量；

步骤二：确定系统的状态向量和量测向量；

步骤2.1建立飞行器姿态估计系统的非线性误差四元数方程，确定系统的状态向量；

非线性误差四元数方程为：

x·=Δρ·Δβ·=-[ω^×]Δρ+12[(Δβ+ηv)×]Δρ-12Δe(Δβ+ηv)ηu,Δe=1-||Δρ||22]]>

其中：为陀螺角速度的估计值，Δe为误差四元数的标量部分，η_v为陀螺仪的测量噪声，η_u为陀螺漂移的测量噪声，[ω×]表示由向量ω的分量构成的反对称矩阵，

[ω×]=0-ω3ω2ω30-ω1-ω2ω10;]]>

状态向量x由误差四元数矢量部分Δρ与陀螺漂移误差Δβ组成，x＝[Δρ^T,Δβ^T]^T；

步骤2.2确定系统的量测向量；

飞行器姿态矩阵为：

A(q)=(q42-ρTρ)I3×3+2ρρT-2q4[ρ×]]]>

其中：q＝[q₁,q₂,q₃,q₄]＝[ρ^T,q₄]为姿态四元数，ρ为四元数向量部分，

星敏感器的输出为：

zki=A(q(tk))r→i+vki=A(δq⊗q^)r→i+vki=A(δq)A(q^)r→i+vki]]>

其中：A(q(t_k))为在t_k时刻真实的姿态矩阵，为星敏感器的参考向量，为零均值的高斯白噪声，i为取不同参考矢量所对应的数字，

系统的量测向量为：

zk=zk1zk2zk3=A(δq)A(q^)r→1A(δq)A(q^)r→2A(δq)A(q^)r→3+vk1vk2vk3=h(xk)+vk]]>

其中：h(x_k)为与状态有关的非线性函数，v_k均值为零、方差为R_k的高斯白噪声；

步骤三：在k-1时刻利用迭代容积卡尔曼滤波估计出k时刻的误差四元数矢量部分以及陀螺漂移误差；

将系统的状态向量x_k和量测向量z_k离散化，得到：

xk=f(xk-1,ω^)+wk-1zk=h(xk)+vk]]>

其中：f(·)和h(·)为系统非线性状态函数和量测函数，系统噪声w_k-1与量测噪声v_k分别是均值为零，协方差为Q_k-1和R_k的互不相关的高斯白噪声，

步骤3.1求取容积点；

容积点与容积点的权值：

ϵj=m2[1]j,ωj=1m,j=1,...,m]]>

式中：m为容积点总数，且m＝2n，ε_j为第j个容积点，e＝[1,0,...,0]^T，符号[1]表示对e的元素进行全排列和改变元素符号所产生的点集，[1]_j表示点集中[1]的第j个点；ω_j为对应第j点的权值；

步骤3.2对迭代容积卡尔曼滤波器进行时间更新；

为k-1时刻的状态估计值，P_k-1为方差，容积点及容积点经状态方程的传递值为：

χj,k-1=x^k-1+Sk-1ϵj,j=1,2,...,2n]]>

χj,k*=f(χj,k-1)]]>

其中P_k-1＝S_k-1(S_k-1)^T，

一步预测的状态和协方差阵为：

xk-=Σj=1mωjχj,k*]]>

Pk-=Σj=1mωjχj,k*(χj,k*)T-xk-(xk-)T+Qk-1;]]>

步骤3.3进行量测更新，确定k时刻的状态估计值和状态协方差阵；

容积点的迭代更新函数：

J(χj)=argmin||χj-χj,k-||(Pk-)-12+||z-h(χj)||R-12,j=1,2,...,2n]]>

其中：z为量测值，χ_j为需要求的迭代容积点，为已知的容积点，

量测向量线性化近似的协方差和方差为：

Pxzi≈Pk-(Hji)T,Pzzi≈HjiPk-(Hji)T+Rk]]>

容积点的迭代更新函数可以求得：

χji+1=χj,k-+Pxzi(Pzzi)-1[z-h(χji)-(Pxzi)T(Pk-)-1(χj,k--χji)],j=1,...,2n,]]>

将一步预测的状态与量测噪声进行扩维，则有以及h^a(x_k,v_k)＝h(x_k)+v_k，

求取扩维后的容积点：

χj,k-=x^k0+Sk0ϵj,j=1,2,...,2(n+p)]]>

式中：Pk0=Sk0(Sk0)T,]]>

以和为初始值进行迭代，i＝1,...,N_max，第i次迭代的容积点和方差分别为和

第i次迭代容积点经量测方程的传递值为：

Zj,ki-1=ha(χji-1)]]>

第i次迭代的量测预测、方差及协方差为：

z^ki=Σj=12(n+p)ωjZj,ki-1]]>

Pzzi=12(n+p)Σi=12(n+p)Zj,ki-1(Zj,ki-1)T-z^ki(z^ki)T]]>

Pxzi=12(n+p)Σi=12(n+p)χj,ki-1(Zj,ki-1)T-x^ki-1(z^ki)T]]>

第i次迭代的容积点以及方差为：

χji=χj,k-+Pxzi(Pzzi)-1[z-ha(χji-1)-(Pxzi)T(Pk0)-1(χj,k--χji-1)],j=1,...,2n]]>

Pki=Pk0-Pxzi(Pzzi)-1(Pxzi)T]]>

通过容积点计算第i次迭代的状态估计为：

x^ki=Σj=12(n+p)ωjχji]]>

迭代终止条件为：

||x^ki(1:n)-x^ki-1(1:n)||≤ϵ]]>或i＝N_max

迭代终止时的迭代次数为N，k时刻的状态估计与方差估计为：

xk=x^kN(1:n)]]>

Pk=PkN(1:n,1:n);]]>

步骤四：对姿态及陀螺漂移进行校正；

对于k时刻的估计利用约束条件||δq||₂＝1，求得误差四元数δq_k，求出四元数估计及陀螺漂移估计分别为对姿态及陀螺漂移进行校正，其中为k-1时刻估计值积分预报k时刻的值，从而得到了修正后k时刻姿态及陀螺漂移，同时确定角速度的估计值为

步骤五：姿态估计非线性离散系统的运行时间为M，若k＝M，则输出姿态及陀螺漂移的结果；若k＜M，则重复步骤三至步骤四，直至姿态估计系统运行结束。