CN113048979A - 一种组合导航滤波方法 - Google Patents

Abstract

本发明公开了一种组合导航滤波方法，包括以下步骤：步骤1、针对非线性环境，建立组合导航模型，包括组合导航系统的状态方程及量测方程：步骤2、运用改进的强跟踪UKF算法对组合导航模型的参数进行滤波，所述改进的强跟踪UKF算法基于输出残差序列的协方差矩阵、非线性状态函数和量测函数的雅克比矩阵计算渐消因子τ_k；再结合渐消因子τ_k更新增益矩阵K_k，状态向量估计值

和协方差矩阵P_k。本发明能提高组合导航系统在非线性环境下的滤波稳定性和鲁棒性。

Description

一种组合导航滤波方法

技术领域

本发明主要涉及导航技术领域，具体涉及一种组合导航滤波方法。

背景技术

单一的导航技术都有各自的优缺点，单独使用一种导航技术难以满足用户与日俱增的导航需求。在此基础上，组合导航应运而生。在组合导航领域，滤波算法一直作为一个核心技术被国内外广大学者研究探索，其中强跟踪UKF(STUKF)算法提高了UKF(UnscentedKalman filter,无迹卡尔曼滤波)算法的滤波跟踪性能，提高了组合导航系统对于突变状态的实时跟踪能力，但现有的强跟踪UKF算法计算比较复杂，滤波稳定性较差，导致组合导航定位精度较低。

发明内容

本发明所要解决的技术问题是，针对现有技术的不足，提供一种组合导航滤波方法，提高组合导航系统在非线性环境下的滤波稳定性和鲁棒性。

本发明所提供的技术方案为：

一种组合导航滤波方法，包括以下步骤：

步骤1、针对非线性环境，建立建立组合导航模型，包括组合导航系统的状态方程及量测方程，其中状态函数和量测函数均为非线性函数；

步骤2、运用改进的强跟踪UKF算法对组合导航模型的参数进行滤波(估计)；其中改进的强跟踪UKF算法基于输出残差序列的协方差矩阵、非线性状态函数和量测函数的雅克比矩阵计算渐消因子τ_k；再结合渐消因子τ_k更新增益矩阵K_k，状态向量的估计值

和协方差矩阵P_k。

进一步地，所述步骤2中，渐消因子τ_k计算公式如下：

其中，V_k为输出残差序列Δ_k/k-1的协方差矩阵，输出残差序列

其中z_k为组合导航系统的量测向量，

为量测向量的估计值；Φ_k/k-1和H_k分别为非线性状态函数和量测函数的雅克比矩阵，tr[]为矩阵的迹，R_k为量测噪声的正定方差矩阵；τ_0,k为中间变量。

有益效果：

本发明运用改进的跟踪UKF算法对相对于现有的组合导航滤波算法，具有更强的非线性性；将渐消因子作用于预测协方差矩阵，提高了强跟踪滤波的稳定性，在高动态的复杂环境中，组合滤波表现优异；基于输出残差序列的协方差矩阵、非线性状态函数和量测函数的雅克比矩阵计算渐消因子，进一步提高了估计精度。

附图说明

图1为本发明一实施例提供的组合导航滤波方法的流程示意图

具体实施方式

以下结合附图和具体实施方式对本发明进行进一步具体说明。

本实施例提供一种组合导航滤波方法，包括以下步骤：

步骤1、针对非线性环境，建立如下组合导航模型，包括组合导航系统的状态方程及量测方程：

x_k+1＝f(x_k,w_k)

z_k＝h(x_k,v_k)

其中，x_k和z_k分别为组合导航系统的状态向量和量测向量；组合导航系统的状态向量由组合导航系统的误差数据构成，如位置误差、速度误差等；组合导航系统的量测向量由组合导航系统的量测数据构成，如位置量测值、速度量测值等；f()和h()分别为非线性状态函数和量测函数；w_k和v_k分别为系统噪声(状态过程噪声)和量测噪声；

步骤2、运用改进的强跟踪UKF算法对组合导航模型的参数进行滤波，其中改进的强跟踪UKF算法基于输出残差序列的协方差矩阵、非线性状态函数和量测函数的雅克比矩阵计算渐消因子τ_k；再结合渐消因子τ_k更新增益矩阵K_k，状态向量的估计值

和协方差矩阵P_k，变量中的下标k为时刻编号，完成一轮滤波过程。

进一步地，所述步骤2中，运用改进的强跟踪UKF算法对组合导航模型的参数进行滤波具体包括以下步骤：

(1)对状态向量x和协方差P进行初始化设置：

其中，

为初始时刻，即k＝0时刻状态向量的估计值，x₀为初始时刻的状态向量，其高斯随机向量；E()为期望值，T为转置矩阵，P₀为初始时刻的协方差矩阵；

(2)对状态向量进行采样，获得k-1时刻的Sigma定性采样点(UT变换)，以及各个采样点的一阶权值和二阶权值；

通过采样公式计算k-1时刻的Sigma定性采样点(UT变换)，所述采样公式为：

其中，ξ_i,k-1为k-1时刻的第i个Sigma定性采样点，k＝1,2,...，i＝0,1,...2w，w为系统状态向量的维数；

为k-1时刻的系统状态向量估计值；中间参数λ＝α²(w+K)-w，其中K为比例参数，通常取为0或3-w；α为正值的比例缩放因子，用于调整Sigma定性采样点与

的距离，α的取值范围为[0,1]；

利用一阶权值计算公式和二阶权值计算公式对Sigma定性采样点进行一阶权值和二阶权值的计算，所述一阶权值计算公式为：

其中，W_i ^m为第i个Sigma定性采样点的一阶权值；

所述二阶权值计算公式为：

其中，W_i ^c为第i个Sigma定性采样点的二阶权值，β为非负权系数，高斯分布下β的最佳值为2。

(3)时间更新：对所述Sigma定性采样点、采样点预测均值、协方差进行更新；

根据第一更新公式对所述Sigma定性采样点进行更新，所述第一更新公式为：

γ_i，k/k-1＝f(ξ_i,k-1)

其中，γ_i，k/k-1为更新后的采样点，f(ξ_i,k-1)为非线性变换函数；

根据更新后的Sigma定性采样点和第二更新公式更新状态量预测均值，所述第二更新公式为：

其中，

为状态量预测均值；

根据更新后的Sigma定性采样点和第三更新公式更新状态量预测协方差，所述第三更新公式为：

其中，P_k/k-1为状态量预测协方差，Q_k-1为系统噪声的非负定方差矩阵；

(4)计算量测向量的估计值

和残输出差序列Δ_k/k-1；

z_i，k/k-1＝h(ξ_i,k-1)

其中，z_k为量测向量，其元素为组合导航系统的量测值，如位置、速度的量测值；

(5)计算渐消因子τ_k

其中，V_k为输出残差序列的协方差矩阵，Φ_k/k-1和H_k分别为非线性状态函数和量测函数的雅克比矩阵，tr[]为矩阵的迹，R_k为量测噪声的正定方差矩阵；τ_0,k为中间变量；

(6)利用渐消因子对量测数据的协方差进行更新，所述第四更新公式为：

其中，P_z,k为量测数据的协方差。

(7)量测更新：计算增益矩阵K_k，k时刻状态向量的估计值

和协方差矩阵P_k，即完成了一个周期的滤波过程。

(8)令k＝k+1，返回步骤(2)，进行下一周期的滤波过程。

本发明运用改进的跟踪UKF算法对相对于现有的组合导航滤波算法，具有更强的非线性性；将渐消因子作用于预测协方差矩阵，提高了强跟踪滤波的稳定性，在高动态的复杂环境中，组合滤波表现优异，提高了估计精度；并且在一次状态估计过程中仅使用一次UT(无迹变换)，有效地降低了强跟踪UKF的复杂度，计算相对简单，提高了组合导航计算的效率。

Claims (2)

1.一种组合导航滤波方法，其特征在于，包括以下步骤：

步骤1、针对非线性环境，建立组合导航模型，包括组合导航系统的状态方程及量测方程，其中状态函数和量测函数均为非线性函数；

步骤2、运用改进的强跟踪UKF算法对组合导航模型的参数进行滤波，其中改进的强跟踪UKF算法基于输出残差序列的协方差矩阵、非线性状态函数和量测函数的雅克比矩阵计算渐消因子τ_k；再结合渐消因子τ_k更新增益矩阵K_k，状态向量的估计值

和协方差矩阵P_k。

2.根据权利要求1所述的组合导航滤波方法，其特征在于：所述步骤2中，渐消因子τ_k计算公式如下：

其中，V_k为输出残差序列Δ_k/k-1的协方差矩阵，输出残差序列

其中z_k为组合导航系统的量测向量，

为量测向量的估计值；Φ_k/k-1和H_k分别为非线性状态函数和量测函数的雅克比矩阵，tr[]为矩阵的迹，R_k为量测噪声的正定方差矩阵；τ_0,k为中间变量。

Images