CN110912535B - 一种新型无先导卡尔曼滤波方法 - Google Patents

Abstract

本发明公开了一种新型无先导卡尔曼滤波方法，该方法包括：S1：建立系统基本方程；S2：对系统进行初始化滤波；S3：计算2n+1个Sigma采样点；S4：计算2n+1个Sigma采样点的一步预测；S5：对系统进行时间更新；S6：迭代变量更新；S7：计算卡尔曼增益；S8：对系统状态进行更新和对协方差进行更新，并重复步骤S3至S7，直至滤波结束。应用本发明能够有效提高UKF滤波精度及鲁棒性。

Description

一种新型无先导卡尔曼滤波方法

技术领域

本发明涉及滤波技术领域，特别是涉及一种新型无先导卡尔曼滤波方法。

背景技术

Kalman(卡尔曼)滤波器自提出并被成功运用到“阿波罗登月计划”之后，由于其滤波性能较好和运算复杂度较低被广泛地运用到工程实践应用中，如故障检测、目标跟踪、卫星导航、飞机自动驾驶与定位和通信等领域。经典Kalman理论适用于线性系统，而现实世界基本都是非线性系统。因此，科学家们相继提出了适用于非线性系统的卡尔曼滤波理论，如EKF(Extend Kalman Filter)、UKF(Unscented Kalman Filter)。由于EKF计算量大而UKF精度较高、不要求系统可微、不要求计算Jacobi矩阵，这使得UKF较之EKF性能更强，因此，UKF被广泛地应用到工程实践中。

随着UKF应用的深入，其在随着状态变量维度增加时滤波精度会有所下降或者鲁棒性较低等问题变得越来越明显。因此，越来越多的研究者使用不同的方法改进UKF算法以应用于不同的工程领域。侍壮飞等人在《改进的UKF算法估算锂离子电池SOC》中改进UKF算法将其运用到锂电子电池，虽提高了估算精度，但会出现电压降跌和辨识参数误差增大等问题，雷晓云等人在《基于改进无迹卡尔曼滤波的弹道射程修正算法研究》中利用牛顿插值法改进UKF算法将其应用到导弹射程修正系统中，虽然降低了GPS数据造成的修正误差但仍不能满足高精度弹道射程修正弹的要求。因此，以上方法都不适用于更复杂的环境的场合。

发明内容

本发明主要解决的技术问题是提供一种新型无先导卡尔曼滤波方法，能够有效提高UKF滤波精度及鲁棒性。

为解决上述技术问题，本发明采用的一个技术方案是：提供一种新型无先导卡尔曼滤波方法，包括以下步骤：S1：建立系统基本方程，其表达式如下：

x_k＝f(x_k-1)+w_k-1 (1)

z_k＝h(x_k)+v_k (2)

式中，x_k∈Rⁿ表示k时刻的状态向量，z_k∈Rⁿ表示k时刻的量测向量，f(·)和h(·)分别为已知的非线性系统状态方程传递函数和非线性系统量测方程传递函数，过程噪声w_k-1∈Rⁿ和量测噪声v_k∈Rⁿ为不相关的高斯白噪声且两者都为0，协方差分别为Q_k-1和R_k；

S2：对系统进行初始化滤波

设定状态估计与估计误差方差矩阵的初始值，其表达式分别如下；

S3：计算2n+1个Sigma采样点

计算k时刻状态估计的2n+1个Sigma采样点，n是状态变量的维数，计算公式为：

其中，表示矩阵(n+λ)P_x的平方根矩阵的第i列，λ＝α²(n+k)-n决定了采样点与/>的距离，α为调节参数，k为另一个常数参数，通常取为3-n，影响高阶误差的均值和协方差的因素，/>表示状态向量的平均值；

S4：计算2n+1个Sigma采样点的一步预测

采样点一步预测信息的计算公式为：

X_k|k-1＝f(χ_k-1) (6)

S5：对系统进行时间更新

计算系统状态变量的一步预测的均值和一步预测的协方差，将采样点的一步预测信息代入非线性传递函数f(·)得到相应的均值和协方差，其中：

一步预测的均值的计算公式为：

其中，表示计算均值所需的权重；

一步预测的协方差的计算公式为：

其中，表示计算协方差所需的权重；

S6：迭代变量更新

利用步骤S5得到一步预测值进行重复UT变换得到新的Sigma点集，并将新的Sigma点集代入系统基本方程得到量测估计值、量测向量的互协方差矩阵和自协方差矩阵，其中：

量测估计值的表达式如下：

量测向量的自协方差的表达式如下：

量测向量的互协方差的表达式如下：

S7：计算卡尔曼增益

根据步骤S6得到的量测向量的自协方差P_zz,k|k-1和量测向量的互协方差P_xz,k|k-1计算卡尔曼增益，计算公式如下：

S8：对系统状态进行更新和对协方差进行更新，并重复步骤S3至S7，直至滤波结束；

状态估计值的表达式如下：

状态估计误差的协方差的表达式如下：

P_k＝P_k|k-1-G_kP_xz,k|k-1G_k ^T (14)

作为本发明的一个优选实施例，量测向量的自协方差的计算过程如下：

以所述系统基本方程在量测更新步骤中重构观测噪声协方差，首先利用对非线性函数进行泰勒公式展开近似，基于该近似用q微分的Huber方法获得状态估计，定义状态预测误差如下；

设定e_k为k时刻状态预测的误差，构造如下的非线性递归模型：

定义：

其中，S_k＝diag([R_xP_k|k-1])，则对公式(16)进行改写：

其中，

M估计的代价函数定义为：

其中，r_i为残差向量r_k＝y_k-g(x_k)的第i个向量，n和m分别为状态和观测的维度，

代价函数为：

其中，μ为调解因子，取值为1.345，

将代替M估计代价函数中的r_k，用q微分对代价函数求偏导得到代价函数最小值：

定义函数得到/>调整协方差矩阵再通过截取/>得到修正后的量测协方差，其表达式为：

将更新后的代入到UKF的量测更新中，得到量测向量的自协方差。

区别于现有技术的情况，本发明的有益效果是：能够有效提高UKF滤波精度及鲁棒性，可以有效弥补现有UKF算法中存在的误差大、精度不符合要求等不足，可以较好应用于机动目标跟踪CA模型。

附图说明

图1是本发明实施例的新型无先导卡尔曼滤波方法的流程示意图。

图2是本发明实施例的新型无先导卡尔曼滤波方法的FPGA实现框图。

具体实施方式

下面将结合本发明实施例中的附图，对本发明实施例中的技术方案进行清楚、完整地描述，显然，所描述的实施例仅是本发明的一部分实施例，而不是全部的实施例。基于本发明中的实施例，本领域普通技术人员在没有做出创造性劳动前提下所获得的所有其他实施例，都属于本发明保护的范围。

参阅图1，是本发明实施例的新型无先导卡尔曼滤波方法的流程示意图。本实施例的新型无先导卡尔曼滤波方法包括以下步骤：

S1：建立系统基本方程，其表达式如下：

x_k＝f(x_k-1)+w_k-1 (1)

z_k＝h(x_k)+v_k (2)

式中，x_k∈Rⁿ表示k时刻的状态向量，z_k∈Rⁿ表示k时刻的量测向量，f(·)和h(·)分别为已知的非线性系统状态方程传递函数和非线性系统量测方程传递函数，过程噪声w_k-1∈Rⁿ和量测噪声v_k∈Rⁿ为不相关的高斯白噪声且两者都为0，协方差分别为Q_k-1和R_k；

S2：对系统进行初始化滤波

设定状态估计与估计误差方差矩阵的初始值，其表达式分别如下；

S3：计算2n+1个Sigma采样点

计算k时刻状态估计的2n+1个Sigma采样点，n是状态变量的维数，计算公式为：

其中，表示矩阵(n+λ)P_x的平方根矩阵的第i列，λ＝α²(n+k)-n决定了采样点与/>的距离，α为调节参数，k为另一个常数参数，通常取为3-n，影响高阶误差的均值和协方差的因素，/>表示状态向量的平均值；

S4：计算2n+1个Sigma采样点的一步预测

采样点一步预测信息的计算公式为：

X_k|k-1＝f(χ_k-1) (6)

S5：对系统进行时间更新

计算系统状态变量的一步预测的均值和一步预测的协方差，将采样点的一步预测信息代入非线性传递函数f(·)得到相应的均值和协方差，其中：

一步预测的均值的计算公式为：

其中，表示计算均值所需的权重；

一步预测的协方差的计算公式为：

其中，表示计算协方差所需的权重；

S6：迭代变量更新

利用步骤S5得到一步预测值进行重复UT变换得到新的Sigma点集，并将新的Sigma点集代入系统基本方程得到量测估计值、量测向量的互协方差矩阵和自协方差矩阵，其中：

量测估计值的表达式如下：

量测向量的自协方差的表达式如下：

量测向量的互协方差的表达式如下：

S7：计算卡尔曼增益

根据步骤S6得到的量测向量的自协方差P_zz,k|k-1和量测向量的互协方差P_xz,k|k-1计算卡尔曼增益，计算公式如下：

S8：对系统状态进行更新和对协方差进行更新，并重复步骤S3至S7，直至滤波结束

状态估计值的表达式如下：

状态估计误差的协方差的表达式如下：

P_k＝P_k|k-1-G_kP_xz,k|k-1G_k ^T (14)

在本实施例中，量测向量的自协方差的计算过程如下：

以所述系统基本方程在量测更新步骤中重构观测噪声协方差，首先利用对非线性函数进行泰勒公式展开近似，基于该近似用q微分的Huber方法获得状态估计，定义状态预测误差如下；

设定e_k为k时刻状态预测的误差，构造如下的非线性递归模型：

定义：

其中，S_k＝diag([R_xP_k|k-1])，则对公式(16)进行改写：

其中，

M估计的代价函数定义为：

其中，r_i为残差向量r_k＝y_k-g(x_k)的第i个向量，n和m分别为状态和观测的维度，

代价函数为：

其中，μ为调解因子，取值为1.345，

将代替M估计代价函数中的r_k，用q微分对代价函数求偏导得到代价函数最小值：

定义函数得到/>调整协方差矩阵再通过截取/>得到修正后的量测协方差，其表达式为：

将更新后的代入到UKF的量测更新中，得到量测向量的自协方差。

随着UKF应用的深入，其在随着状态变量维度增加时滤波精度会有所下降或者鲁棒性较低等问题变得越来越明显。因此，本方法重构观测噪声协方差，利用q微分和Huber估计理论，在UKF算法的基础上将量测更新方式进行改进。

本实施例的方法可以通过FPGA实现，FPGA实现框图如图2所示。其中，CA模型验证主模块11作为系统控制端，系统运行时，当数据缓存模块12中有数据且完成初始化时CA模型验证主模块11发送Sigma和Init_Done信号给Sigma采样模块13。此时，Sigma采样模块13从数据缓存模块12读取状态信息以及量测信息进行采样处理得到采样点χ_k-1并存储到数据缓存模块12，发送Sigma_Done信号到非线性函数计算模块14。当非线性函数计算模块14接收到Sigma_Done信号时，开始读取采样点经系统过程函数f(·)进行处理得到的2n+1个Sigma采样点的一步预测值X_k|k-1，并向CA模型验证主模块11发送pre信号。由CA模型验证主模块11发送pre信号到时间更新模块15进行运算得到系统状态一步预测值和协方差矩阵P_k|k-1，并发送pre_Done到CA模型验证主模块11。由CA模型验证主模块11发送Sigma信号到Sigma采样模块13，将得到的状态预测值和协方差矩阵发送到非线性函数计算模块14计算得到新的Sigma点集，并将得到的新的Sigma点集发送到非线性函数计模块14计算得到观测值Z_k|k-1，将观测值进行运算得到量测向量的自协方差P_zz,k|k-1和互协方差P_xz,k|k-1，然后利用得到的量测向量的协方差计算卡尔曼增益G_k。由CA模型验证主模块11发送update信号到量测更新模块16并得到更新后的状态估计/>和状态估计误差的协方差P_k。以上过程为一次卡尔曼滤波过程。系统由CA模型验证主模块11判断滤波是否结束，如果滤波未结束进入状态估计信息、量测信息发送状态。CA模型验证主模块11将依据非线性函数计算模块14的控制信号控制数据缓存模块12、Sigma采样模块13、时间更新模块15以及量测更新模块16的完成，继而实现应用仿真。

以上所述仅为本发明的实施例，并非因此限制本发明的专利范围，凡是利用本发明说明书及附图内容所作的等效结构或等效流程变换，或直接或间接运用在其他相关的技术领域，均同理包括在本发明的专利保护范围内。

Claims (1)

1.一种机动目标跟踪系统，其特征在于，包括FPGA，FPGA实现新

型无先导卡尔曼滤波方法，包括以下步骤：S1：建立系统基本方

程，其表达式如下：x_k＝f(x_k-1)+w_k-1 (1)

z_k＝h(x_k)+v_k (2)

式中，x_k∈Rⁿ表示k时刻的状态向量，z_k∈Rⁿ表示k时刻的量测向量，f(·)和h(·)分别为已知的非线性系统状态方程传递函数和非线性系统量测方程传递函数，过程噪声w_k-1∈Rⁿ和量测噪声v_k∈Rⁿ为不相关的高斯白噪声且两者都为0，协方差分别为Q_k-1和R_k；

S2：对系统进行初始化滤波

设定状态估计与估计误差方差矩阵的初始值，其表达式分别如下；

S3：计算2n+1个Sigma采样点

计算k时刻状态估计的2n+1个Sigma采样点，n是状态变量的维数，计算公式为：

其中，表示矩阵(n+λ)P_x的平方根矩阵的第i列，λ＝α²(n+k)-n决定了采样点与/>的距离，α为调节参数，k为另一个常数参数，通常取为3-n，影响高阶误差的均值和协方差的因素，/>表示状态向量的平均值；

S4：计算2n+1个Sigma采样点的一步预测

采样点一步预测信息的计算公式为：

X_k|k-1＝f(χ_k-1) (6)

S5：对系统进行时间更新

计算系统状态变量的一步预测的均值和一步预测的协方差，将采样点的一步预测信息代入非线性传递函数f(·)得到相应的均值和协方差，其中：

一步预测的均值的计算公式为：

其中，表示计算均值所需的权重；

一步预测的协方差的计算公式为：

其中，表示计算协方差所需的权重；

S6：迭代变量更新

利用步骤S5得到一步预测值进行重复UT变换得到新的Sigma点集，并将新的Sigma点集代入系统基本方程得到量测估计值、量测向量的互协方差矩阵和自协方差矩阵，其中：

量测估计值的表达式如下：

量测向量的自协方差的表达式如下：

量测向量的互协方差的表达式如下：

S7：计算卡尔曼增益

根据步骤S6得到的量测向量的自协方差P_zz,k|k-1和量测向量的互协方差P_xz,k|k-1计算卡尔曼增益，计算公式如下：

S8：对系统状态进行更新和对协方差进行更新，并重复步骤S3至S7，直至滤波结束

状态估计值的表达式如下：

状态估计误差的协方差的表达式如下：

P_k＝P_k|k-1-G_kP_xz,k|k-1G_k ^T (14)

所述量测向量的自协方差的计算过程如下：

以所述系统基本方程在量测更新步骤中重构观测噪声协方差，首先利用对非线性函数进行泰勒公式展开近似，基于该近似用q微分的Huber方法获得状态估计，定义状态预测误差如下；

设定e_k为k时刻状态预测的误差，构造如下的非线性递归模型：

定义：

其中，S_k＝diag([R_x P_k|k-1])，则对公式(16)进行改写：

其中，

M估计的代价函数定义为：

其中，r_i为残差向量r_k＝y_k-g(x_k)的第i个向量，n和m分别为状态和观测的维度，

代价函数为：

其中，μ为调解因子，取值为1.345，

将代替M估计代价函数中的r_k，用q微分对代价函数求偏导得到代价函数最小值：

定义函数得到/>调整协方差矩阵/>再通过截取/>得到修正后的量测协方差，其表达式为：

将更新后的代入到UKF的量测更新中，得到量测向量的自协方差；FPGA包括CA模型验证主模块、数据缓存模块、Sigma采样模块、非线性函数计算模块、时间更新模块、量测更新模块；

CA模型验证主模块，作为系统控制端，当数据缓存模块中有数据且完成初始化时，CA模型验证主模块发送Sigma和Init_Done信号给Sigma采样模块；此时，Sigma采样模块从数据缓存模块读取状态信息以及量测信息进行采样处理得到采样点X_k-1，并存储到数据缓存模块，发送Sigma_Done信号到非线性函数计算模块；当非线性函数计算模块接收到Sigma_Done信号时，开始读取采样点经系统过程函数f(·)进行处理得到的2n+1个Sigma采样点的一步预测值X_k|k-1，并向CA模型验证主模块发送pre信号；由CA模型验证主模块发送pre信号到时间更新模块进行运算得到系统状态一步预测值和协方差矩阵P_k|k-1，并发送pre_Done到CA模型验证主模块；由CA模型验证主模块发送Sigma信号到Sigma采样模块，将得到的状态预测值和协方差矩阵发送到非线性函数计算模块，计算得到新的Sigma点集，并将得到的新的Sigma点集发送到非线性函数计模块计算得到观测值Z_k|k-1，将观测值进行运算得到量测向量的自协方差P_zz，k|k-1和互协方差P_xz，k|k-1，然后利用得到的量测向量的协方差计算卡尔曼增益G_k；由CA模型验证主模块发送update信号到量测更新模块并得到更新后的状态估计和状态估计误差的协方差P_k，完成卡尔曼滤波；由CA模型验证主模块判断滤波是否结束，如果滤波未结束，进入状态估计信息、量测信息发送状态；CA模型验证主模块依据非线性函数计算模块的控制信号控制数据缓存模块、Sigma采样模块、时间更新模块以及量测更新模块的完成，实现应用仿真。