CN103575298A - 基于自调节的ukf失准角初始对准方法 - Google Patents

Abstract

本发明公开了一种基于自调节的UKF失准角初始对准方法，包括：（1）根据惯导初始对准的误差特性，列写出滤波的状态方程与量测方程；（2）设置滤波初值x₀和P₀；（3）确定调节参数的最优值

（4）在

的条件下，根据所述非线性误差模型确定滤波状态的均值

和协方差P_k|k；（5）在

的条件下，根据所述非线性误差模型确定滤波状态的预测均值x_k+1|k和协方差P_k+1|k，并使用x_k+1|k和P_k+1|k对所述失准角进行对准。本发明能够有效的估计出载体系与导航系之间的失准角误差，为导航定位提供可靠的精度。

Description

基于自调节的UKF失准角初始对准方法

技术领域

本发明涉及滤波领域，尤其涉及一种基于自调节的UKF失准角初始对准方法。 

背景技术

状态估计的主要任务是如何将系统的状态向量从含有噪声的观测量中估计出来。非线性滤波方法在近几十年来已经成为各行业研究的热点。特别是随着计算机仿真技术的不断发展，非线性滤波理论已得到了很大的发展。工程上常用的非线性滤波方法当属扩展卡尔曼滤波（Extended Kalman Filter，EKF）和无迹卡尔曼滤波（Unscented Kalman Filter，UKF）。但是，EKF存在两个比较大的缺点，致使其应用受到一定的限制。首先，当系统的非线性度比较强时，线性化后导致的滤波误差会比较大，甚至会导致滤波发散。其次，在进行线性化时需要求雅克比矩阵，使得计算量增大。而且，当系统不连续时，也无法应用EKF。 

UKF滤波的核心是无迹（Unscented，UT）变换，它通过采取2n+1个确定性点（即Sigma点，2n个点的权值为1/2(n+κ)，1个点的权值为κ/n+κ），经非线性映射后逼近高斯正态变量的均值和协方差。这种非线性变换方法的精度能够达到非线性系统泰勒展开式的三阶项以上。 

自由调节参数κ对于滤波的精度和稳定性会有很大的影响。但是如何设置κ却没有一个严格的理论进行说明。通常认为，如果状态变量服从高斯正态分布时，应该满足n+κ＝3。这样，非线性变换能够捕获系统大部分的四阶距信息。随着容积卡尔曼滤波（Cubature Kalman Filter，CKF）滤波的产生，滤波参数的选取更是成为了人们关注的焦点。因为从滤波方法上来讲，CKF滤波就是UKF滤波在κ取0时的一种应用。通过理论分析和实验仿真也表明当系统为3维及以下时，应选取UKF滤波；当系统为3维以上时应选取CKF滤波。但是，对于不同的非线性函数g(x)，由于非线性度的不同，可能会导致在进行泰勒级数展开时一些高阶矩信息对UT变换的影响会大于大部分的4阶矩信息。这样，有可能一些高阶矩信息对于变换后统计均值和方差的求取是十分有利的。因此，κ的选取和系统模型本身具有很大的相关性的，而且κ在每一次滤波时都应该有一个最优值。 

发明内容

本发明提供了一种基于自调节的UKF失准角初始对准方法，用于解决现有技术中UKF滤波鲁棒性差的问题。 

一方面，提供了一种基于自调节的UKF大失准角初始对准新方法，包括： 

步骤一、根据惯导的误差特性，建立滤波的非线性误差模型，即状态方程和量测方程。 

步骤二、设置滤波初值。令和P_0|-1＝P₀。其中，x₀为状态变量的初值，P₀为状态变量的初始协方差。 

步骤三、根据公式

选取调节参数的最优值

（κ为自由调节参数值），其中，K＝{κ:κ_min∈R,κ_max∈R,κ_min≤κ≤κ_max}是一些固定性的可能取值且满足误差的协方差阵正定。 

步骤四、在

的条件下，获取滤波状态的均值和协方差

P_k|k。 

步骤五、在

的条件下，获取滤波状态的预测均值和协方差

P_k+1|k。 

令k＝k+1，算法继续从步骤三进行；具体算法见图2。 

UKF滤波的基础是Unscented变换（UT变换），这种变换是不同维数线性空间的非线性映射。它通过采取一些固定性的点进行逼近变换后的均值与方差，变换过程见图1。由于在实际的大系统中，很难满足系统的状态变量严格服从高斯正态分布。而且对于滤波模型的建立也很难做到准确无误，总会存在着一定的误差。因此，很难做到事先确定好固定的滤波调节参数κ的值。据此通过κ对滤波量测量的影响能够在线的确定出最优值，从而更加准确的估计出状态变量的均值和协方差。将此发明应用到SINS大失准角初始对准中，能够有效提高滤波的精度、减小估计的失准角误差。 

通过上述方法，本发明具有以下优点： 

一、本发明增加了UKF滤波的鲁棒性，提高了抗干扰能力； 

二、本发明可以使UKF滤波调节参数的选取不在依赖于系统的模型与状态的分布特性； 

三、本发明可以有效的保证UKF滤波应用在捷联惯导系统（Strapdown Intertial Navigation System,SINS）大失准角初始对准中的滤波精度和稳定性。 

附图说明

图1是UT变换原理图； 

图2是自调节UKF滤波的算法示意图； 

图3是方位失准角的估计精度示意图； 

图4是方位失准角的稳态精度示意图。 

具体实施方式

下面结合附图，对本发明的实现方式进行详细说明。 

本发明实施例提供了一种基于自调节的UKF大失准角初始对准新方法，包括： 

步骤一、根据惯导的误差特性，建立滤波的非线性误差模型，非线性误差模型包括：状态方程和量测方程。 

步骤二、设置滤波初值。令

和P_0|-1＝P₀。其中，x₀为状态变量的初值，P₀为状态变量的初始协方差。 

步骤三、根据公式

选取调节参数的最优值

（κ为自由调节参数值），其中，K＝{κ:κ_min∈R,κ_max∈R,κ_min≤κ≤κ_max}是一些固定性的可能取值且满足误差的协方差阵正定。z_k为量测的真值。 

步骤四、在

的条件下，获取滤波状态的均值和协方差

P_k|k。 

步骤五、在的条件下，获取滤波状态的预测均值和协方差

P_k+1|k。 

优选的，所述步骤一中，在惯导初始对准中一般是通过系统的速度误差量去估计失准角的大小，而在粗对准后通常水平失准角是小失准角，方位失准角是大失准角，因此，有必要建立起系统的非线性误差模型。其静基座初始对准的状态方程如下： 

其中，φ_x、φ_y和φ_z为三个方向的失准角（状态量）；δv_x和δv_y为东向和北向速度误差（状态量）；为当地纬度；ω_ie为地球自转角速度；

和

为三个轴向的陀螺漂移；

和

为三个轴向的加速度零偏；f_x、f_y和f_z为加速度输出比例在计算地理系上的值；C_ij是载体系到地理系的方向矩阵中的元素；C'_ij是载体系到计算地理系矩阵中的元素。 

初始对准的量测方程如下： 

Z＝HX+V 

其中，量测量Z为惯导东向和北向速度误差δv_x，δv_y；H取H＝[I_2×2 0_2×3]（I_2×2为2维的单位矩阵；O_2×3为2行3列的零矩阵）；V为量测噪声。 

步骤一包括：在SINS系统的粗对准阶段，可以通过引入惯导的航向姿态信息迅速将数学平台对准到导航坐标系下以减小初始的失准角。而在精对准阶段可以通过多普勒提供速度信息作为外观测量，利用自调节的无迹卡尔曼滤波完成精对准过程。 

所述步骤二中，由于惯导的非线性滤波模型是可观测且可控的，因此滤波初值的选取不会影响滤波的精度和稳定性。在没有任何先验信息的情况下，可以令初始状态的均值为零，协方差为无穷大。 

在步骤三中，根据量测量的信息去估计最优的κ值。一般来讲并不能准确的知道量测量的分布特性。但是，作为一种近似高斯正态分布的假定是合理的，它符合自然界一种能量集中的原则。而且分布的方差越小，说明各个量测量越接近真值。因此，这里在利用量测量确定最优的κ值时，只要保证量测量的方差最小即可。 

步骤三包括以下步骤： 

步骤三一：确定UKF滤波的误差特性。 

假定任意分布的随机变量y的均值为

经UT变换的近似值为

将它们之间的误差记为

则有 

$\tilde{y}=\hat{y}-{\hat{y}}^{\mathrm{UKF}}=\∫g\left(x\right)p\left(x\right)\mathrm{dx}-\underset{i=0}{\overset{{2n}_x}{\mathrm{\Σ}}}{w}_{i}g\left({\mathrm{\χ}}_i\right)---\left(1\right)$

这里，p(x)为随机变量x的概率密度函数。其中均值为

协方差阵为P_x。此时，对非 线性函数g(x)进行泰勒级数展开，有 

$\begin{array}{c}g\left(x\right)=g\widehat{\left(x\right)}+\underset{j=1}{\overset{\∞}{\mathrm{\Σ}}}\frac{1}{j!}{(I_{n_x}\⊗{(x-\hat{x})}^{\⊗j-1})}^T\\ \×{\left(\frac{d^{j}g\left(x\right)}{{\mathrm{dx}}^j}\right)}^T{|}_{x=\hat{x}}(x-\hat{x})\end{array}---\left(2\right)$

这里，

为矩阵A和B的克罗尼克积。

代表A的n次克罗尼克幂。同时，g(x)对向量x的k次导数定义若如下： 

$\frac{d^{k}g\left(x\right)}{{\mathrm{dx}}^k}=\frac{\∂}{\∂\mathrm{vecx}}{\mathrm{vec}}^{T}g\⊗\frac{\∂}{\∂\mathrm{ve}{c}^{T}x}\⊗...\frac{\∂}{\∂{\mathrm{vec}}^{T}x}\⊗---\left(3\right)$

vec(A)定义如下： 

$\mathrm{vec}\left(A\right)=\mathrm{vec}(a_1...a_q)=\left[\begin{array}{c}{a}_1\\ .\\ .\\ .\\ a_q\end{array}\right]---\left(4\right)$

其中，A是一个p×q的矩阵，a_i是它的第i列。依据泰勒级数展开式，则UT变换误差如下： 

$\begin{array}{c}\hat{y}=\∫[g\left(\hat{x}\right)+\underset{j=1}{\overset{\∞}{\mathrm{\Σ}}}\frac{1}{j!}{(I_{n_x}\⊗{(x-\hat{x})}^{\⊗j-1})}^T\\ \×{\left(\frac{d^{j}g\left(x\right)}{{\mathrm{dx}}^j}\right)}^T{|}_{x=\hat{x}}(x-\hat{x})\left]N\right\{x:\hat{x},P\}\mathrm{dx}\\ -\underset{j=1}{\overset{\∞}{\mathrm{\Σ}}}{w}_i[g\left(\hat{x}\right)+\underset{j=1}{\overset{\∞}{\mathrm{\Σ}}}\frac{1}{j!}{(I_{n_x}\⊗{({\mathrm{\χ}}_i-\hat{x})}^{\⊗j-1})}^T\\ \×{\left(\frac{d^{j}g\left(x\right)}{{\mathrm{dx}}^j}\right)}^T{|}_{x=\hat{x}}({\mathrm{\χ}}_i-\hat{x})]\end{array}---\left(5\right)$

为了清晰的表达数学变量，记 

$\begin{array}{c}{T}_j\left(x\right)\stackrel{\mathrm{\Δ}}{=}\frac{1}{j!}{(I_{n_x}\⊗{({\mathrm{\χ}}_i-\hat{x})}^{\⊗j-1})}^T\\ \×{\left(\frac{d^{j}g\left(x\right)}{{\mathrm{dx}}^j}\right)}^T{|}_{x=\hat{x}}({\mathrm{\χ}}_i-\hat{x})\end{array}---\left(6\right)$

由于 $\∫g\left(\hat{x}\right)N\{x:\hat{x},P\}\mathrm{dx}=g\left(\hat{x}\right),$ 且 ${\mathrm{\Σ}}_{i=0}^{{2n}_x}{w}_i=1.$

则可以得到误差的表达式，如下： 

$\begin{array}{c}\tilde{y}=\∫\left[\underset{j=1}{\overset{\∞}{\mathrm{\Σ}}}{T}_j\left(x\right)\right]N\{x:\hat{x},P\}\mathrm{dx}-\underset{i=0}{\overset{{2n}_x}{\mathrm{\Σ}}}{w}_i\left[\underset{j=1}{\overset{\∞}{\mathrm{\Σ}}}{T}_j\left({\mathrm{\χ}}_i\right)\right]\\ =\underset{j=1}{\overset{\∞}{\mathrm{\Σ}}}(\∫T_j\left(x\right)N\{x:\hat{x},P\}\mathrm{dx}-\underset{i=0}{\overset{{2n}_x}{\mathrm{\Σ}}}{w}_i{T}_j\left({\mathrm{\χ}}_i\right))\end{array}---\left(7\right)$

根据对称性，各奇阶矩均为零。UT变换的误差可以表达如下： 

$\tilde{y}=\underset{j=1}{\overset{\∞}{\mathrm{\Σ}}}(\∫T_{2l}\left(x\right)N\{x:\hat{x},P\}\mathrm{dx}-\underset{i=0}{\overset{{2n}_x}{\mathrm{\Σ}}}{w}_i{T}_{2l}\left({\mathrm{\χ}}_i\right))---\left(8\right)$

步骤三二：误差特性的定性分析与κ的取值原则。 

根据UT变换的误差表达式，UKF滤波的误差取决于系统的模型与状态变量的分布。按照传统的UT变换中Sigma点的选取与权值的规定，非线性变换后能够精确的捕获到系统的2阶距信息。UT变换的算法框图如图2所示。此时，不同的κ值捕获高阶距信息的能力是不同的。而且，在每一次滤波中κ都应该有一个最优的值，自调节算法正是根据每一次量测后，通过量测信息给出一个最优的值。 

对于κ的取值，一般情况下，κ_min取为0，κ_max不超过9n_x。这样能够保证不忽略非中心权值粒子的影响。 

步骤四：在最优的κ下滤波一步预测均值和方差的求取。 

估计k时刻的状态预测值： 

${\hat{x}}_{k|k-1}=\underset{i=0}{\overset{m}{\mathrm{\Σ}}}{w}_i^m{X}_{i,k|k-1}^{*}---\left(9\right)$

估计k时刻的状态误差协方差预测值： 

$P_{k|k-1}=\underset{i=0}{\overset{m}{\mathrm{\Σ}}}{w}_i^m(X_{i,k|k-1}^{*}-{\hat{x}}_{k|k-1}){(X_{i,k|k-1}^{*}-{\hat{x}}_{k|k-1})}^T+Q_{k-1}---\left(10\right)$

为对应点的权值，

为经非线性变换后的点，Q_k-1为状态噪声的方差阵。 

步骤五：在最优的κ下滤波均值和方差的求取。 

k时刻状态估计值： 

${\hat{x}}_{k|k}={\hat{x}}_{k|k-1}+W_k(z_k-{\hat{z}}_{k|k-1})---\left(11\right)$

W_k为滤波增益矩阵，

为量测的一步预测均值，z_k为量测的真值。 

k时刻状态误差协方差估计值： 

$P_{k|k}=P_{k|k-1}-W_k{P}_{\mathrm{zz},k|k-1}{W}_k^T---\left(12\right)$

其中，P_zz,k|k-1为量测的误差协方差阵。 

为比较传统UKF和自调节UKF的滤波效果，这里进行了一般性的仿真验证。初始条件设定如下：初始经度λ₀＝126.67^°，纬度

初始姿态角：ψ₀＝10°，θ₀＝0°，γ₀＝0^°；初始失准角：φ_x0＝φ_y0＝0.5^°，φ_z＝10^°；初始加速度a＝0m/s，速度v＝0m/s²； 

由于水平失准角为小角度，方位失准角是大角度的，仿真结果给出方位失准角的估 计精度，如图3所示。稳态精度如图4所示。 

通过比较，自调节UKF对方位失准角的估计精度明显由于传统UKF的估计精度。 

上述实施例仅是本发明的优选实施例，并不用于限定本发明的保护范围，在不偏离本发明的实质的情况下，本领域技术人员能够做出各种变型，这些变型也应在本发明的保护范围之内。 

Claims (4)

1.一种基于自调节的无迹卡尔曼滤波UKF失准角初始对准方法，其特征在于，包括：

步骤一、根据惯导的误差特性，建立滤波的非线性误差模型；

步骤二、设置滤波初值x₀和P₀，其中，x₀为状态变量的初值，P₀为状态变量的初始协方差；

步骤三、根据公式

确定调节参数的最优值

其中，

表示对量测量的一步预测概率密度取最大；z_k表示k时刻的量测量，z^k-1为k时刻以前的所有量测量之和，κ为自由调节参数；K为κ的取值范围，且K={κ:κ_min∈R,κ_max∈R,κ_min≤κ≤κ_max}，R为实数域，κ_min为κ取值的下限，κ_max为κ取值上限；

步骤四、在

的条件下，根据所述非线性误差模型确定滤波状态的预测均值

和协方差P_k+1|k；

步骤五、在

的条件下，根据所述非线性误差模型确定滤波状态的均值和协方差P_k|k，并使用

和P_k|k对所述失准角进行对准。

2.根据权利要求1所述的方法，其特征在于，所述步骤一中，在惯导初始对准中，根据系统的速度误差量估计失准角的大小，在粗对准后如果确定水平失准角是小失准角，方位失准角是大失准角，则建立起系统的非线性误差模型。

3.根据权利要求2所述的方法，其特征在于，所述小失准角是小于第一预定角度的失准角，所述大失准角是大于第二预定角度的失准角。

4.根据权利要求1所述的方法，其特征在于，所属步骤二包括：设置x₀为零，P₀为无穷大。

Images