CN112671373A - 一种基于误差控制的卡尔曼滤波自适应滤波算法 - Google Patents

Abstract

本发明公开了一种基于误差控制的卡尔曼滤波自适应滤波算法，考虑到初始滤波计算过程及系统状态突变的场景下，由于观测误差过大，造成滤波结果不稳定的情况，在自适应卡尔曼滤波算法的基础上，基于观测误差的协方差矩阵，设计一个自适应抑制因子，再利用该自适应抑制因子构建观测误差协方差矩阵的自适应修正模型，最后结合卡尔曼滤波算法的预测及更新模型，利用当前时刻系统状态的测量值和预测值，得到系统当前时刻较为精确的最终状态。本发明通过设计自适应抑制因子对系统进行误差控制，降低了观测误差对于卡尔曼增益的影响，提升了基于滤波算法估计系统状态结果的稳定性。

Description

一种基于误差控制的卡尔曼滤波自适应滤波算法

技术领域

本发明涉及无线通信网络技术领域，特别涉及一种基于误差控制的卡尔曼滤波自适应滤波算法。

背景技术

随着自动驾驶技术、航空航天技术、传感技术等不断的发展，对系统状态进行精确估计的相关算法越来越重要，其标准也越来越高，尤其是在精确制导、精确定位、精确温度控制等应用领域，相关算法的研究十分迫切。自上世纪六十年代，卡尔曼滤波算法一经提出，便得到了强烈的关注。经过不断的演进，卡尔曼滤波算法及其衍生算法在许多领域得到了十分广泛的应用。

然而，现阶段人们针对于卡尔曼滤波算法及其衍生算法的研究重点关注于提升滤波结果的精度和算法的计算复杂度等方面，然而较少有人关注到卡尔曼滤波算法初始计算过程的滤波结果不稳定的问题，以及对于状态复杂多变的目标系统，由于其系统状态剧烈变化造成的滤波结果不稳定甚至导致估计结果发散的问题。相关方面的研究对于改善滤波计算结果的稳定性具有十分重要的意义。

发明内容

本发明的目的在于提供一种基于误差控制的卡尔曼滤波自适应滤波算法，以解决现有技术在滤波估计过程当中，由于初始滤波计算时系统状态估计值偏差较大，或者在遇到系统状态突变的情况时，系统的观测误差过大造成滤波计算结果发散等不稳定的问题。

为解决上述技术问题，本发明的实施例提供如下方案：

一种基于误差控制的卡尔曼滤波自适应滤波算法，包括以下步骤：

根据观测误差的协方差矩阵设计自适应抑制因子，并利用所述自适应抑制因子构建观测误差协方差矩阵的自适应修正模型，以降低观测误差对卡尔曼增益的影响；

利用所述自适应抑制因子及所述自适应修正模型对滤波结果进行误差控制，以系统状态实际测量值为基础，结合基于误差控制的卡尔曼自适应滤波算法，对系统状态进行滤波估计，达到提升滤波结果稳定性的目的。

优选地，采用Sage-Husa卡尔曼自适应滤波算法对系统状态进行滤波估计，计算模型为：

预测过程如下：

引入调节更新参量：

更新过程如下：

其中，P_k为估计值与真实值之间的误差协方差矩阵，H为观测矩阵，d_k为引入的调节更新参量，b为遗忘因子，取值范围在0.95-0.99之间，K为卡尔曼增益。

优选地，设计自适应抑制因子的步骤包括：

R_k＝(1-d_k)R_k-1+A+B

其中R_k的两个余项A和B分别为：

为观测噪声协方差R_k引入新的抑制因子：

E＝(A-2B)(A-2B)^T

其中，λ为自适应抑制因子，R_k为当前时刻观测噪声协方差矩阵，A和B为观测噪声协方差矩阵的两个余项，v_k为观测噪声，P_k为先验估计协方差，H为观测矩阵，d_k为引入的调节更新参量，b为遗忘因子，取值范围在0.95-0.99之间，K为卡尔曼增益，η为余项的偏差。

优选地，引入自适应抑制因子的卡尔曼自适应滤波算法的计算模型为：

更新过程如下：

其中，

为系统状态的预测值，F为状态转移矩阵，BU_k-1为系统控制项，s_k为预测噪声，其协方差矩阵为Q，Y_k为系统状态的测量值。

至此，利用系统状态的实际测量值，结合上述引入自适应抑制因子进行误差控制的卡尔曼滤波自适应滤波算法，即可得到当前时刻系统状态的最终估计结果。

本发明实施例提供的技术方案带来的有益效果至少包括：

本发明针对初始滤波计算过程及系统状态突变的场景下，由于观测误差过大，造成滤波结果不稳定的情况，在自适应卡尔曼滤波算法的基础上，基于观测误差的协方差矩阵，设计了一个自适应抑制因子，再利用该自适应抑制因子构建观测误差协方差矩阵的自适应修正模型，最后结合卡尔曼滤波算法的预测及更新模型，利用当前时刻系统状态的测量值和预测值，得到系统当前时刻较为精确的最终状态。本发明通过设计自适应抑制因子对系统进行误差控制，降低了观测误差对于卡尔曼增益的影响，提升了基于滤波算法估计系统状态结果的稳定性。

附图说明

为了更清楚地说明本发明实施例中的技术方案，下面将对实施例描述中所需要使用的附图作简单地介绍，显而易见地，下面描述中的附图仅仅是本发明的一些实施例，对于本领域普通技术人员来讲，在不付出创造性劳动的前提下，还可以根据这些附图获得其他的附图。

图1是本发明实施例提供的一种基于误差控制的卡尔曼滤波自适应滤波算法的流程图；

图2是本发明实施例提供的加入自适应抑制因子前的自适应卡尔曼滤波算法的计算效果示意图；

图3是本发明实施例提供的基于误差控制的卡尔曼滤波自适应滤波算法的计算效果示意图；

图4是本发明实施例提供的基于误差控制的卡尔曼滤波自适应滤波算法与加入自适应抑制因子前的自适应卡尔曼滤波算法计算结果的均方根误差对比情况示意图。

具体实施方式

为使本发明的目的、技术方案和优点更加清楚，下面将结合附图对本发明实施方式作进一步地详细描述。

本发明的实施例提供了一种基于误差控制的卡尔曼滤波自适应滤波算法，如图1所示，该方法包括以下步骤：

根据观测误差的协方差矩阵设计自适应抑制因子，并利用所述自适应抑制因子构建观测误差协方差矩阵的自适应修正模型，以降低观测误差对卡尔曼增益的影响；

利用所述自适应抑制因子及所述自适应修正模型对滤波结果进行误差控制，以系统状态实际测量值为基础，结合基于误差控制的卡尔曼自适应滤波算法，对系统状态进行滤波估计，达到提升滤波结果稳定性的目的。

本发明主要针对在滤波估计过程当中，由于初始滤波计算时系统状态估计值偏差较大，或者在遇到系统状态突变的情况时，系统的观测误差过大会造成滤波估计结果不稳定的问题，提供了一种基于误差控制的卡尔曼滤波自适应滤波算法，通过设计自适应抑制因子，对系统进行误差控制，达到提升系统滤波估计稳定性的目的。

考虑到基于信号到达时间、基于信号到达时间差、基于信号到达角度以及基于接收信号强度等基本测量方法各自具备的优缺点，采用信号到达时间差和信号到达角度相结合的混合定位方法进行位置测量值的计算。其测量计算过程可表示为：

R_T+R_R＝c(t_t+t_r-t_l)+R_L＝c*Δt+R_L

R_T ²＝R_R ²+R_L ²-2R_RR_Lcos(θ)

其中：

R_L ²＝x_b ²+y_b ²

R_T ²＝(x_b-x)²+(y_b-y)²

R_R ²＝x²+y²

由此可得，系统的观测方程为：

系统观测方程可以简写为：

首先由定位基站BS同时发送两个信号，其中一个信号直接到达基准站接收终端RSU，信号传播所用时间为t_l，定位基站BS与接收终端RSU之间的距离为R_L。另一个信号从定位基站BS发出后，先由移动终端MS接收后再转发给接收终端RSU，其中BS与MS间距离为R_T，信号传播时间为t_t，移动终端MS与RSU之间的距离为R_R，信号传播时间为t_r，电磁波传播速度为c。

此外RSU测得两个接收信号之间的夹角为θ。设RSU位置为(0,0)，BS坐标为(x_b，y_b)，移动终端MS坐标为(x，y)。ω_k＝[ω_t ω_θ]^T，其中ω_t是系统的时间测量误差，ω_θ是系统的角度测量误差，在理想情况下，均为高斯白噪声。Y_k＝[Δt_k θ_k]^T是k时刻系统的观测结果。

考虑到只采用一个定位基站和一个基准站配合进行定位信息测量其结果可靠性较低的问题，采用三个定位基站与基准站同时配合进行信息测量的方式，能够保证更高的可信度。可以将定位信息测量过程表示为：

系统观测方程可以简写为：

其中Y_k,i＝[Δt_i θ_i]^T是第i个定位基站k时刻的测量结果。由此可以分别得到三个位置坐标测量值。然后采用数学计算方法—三角形取质心，对移动终端位置坐标测量值的最终结果进行估计，可表示为：

在得到移动终端位置坐标的测量值之后，需要利用相关的滤波算法对其进行更为准确的滤波估计，以得到最终的精度较高的定位结果。

采用Sage-Husa自适应卡尔曼滤波算法对移动终端的位置坐标进行滤波估计，其计算模型可表述为：

预测过程如下：

引入调节更新参量：

更新过程如下：

其中，P_k为估计值与真实值之间的误差协方差矩阵，H为观测矩阵，d_k为引入的调节更新参量，b为遗忘因子，取值范围在0.95-0.99之间，K为卡尔曼增益。随着计算次数的增加，观测噪声协方差R_k逐渐趋于稳定。

考虑到初始滤波计算过程及移动终端运动状态突变的场景下，由于观测误差过大，造成滤波结果不稳定的情况，引入自适应抑制因子，抑制观测误差对滤波结果产生的影响，从而达到提升滤波结果稳定性的目的。

分析自适应卡尔曼滤波算法的推导过程可知：滤波结果发散主要缘于观测噪声偏大，导致观测噪声协方差不稳定。基于上述分析对抑制因子进行设计：

R_k＝(1-d_k)R_k-1+A+B

其中R_k的两个余项A和B分别为：

为观测噪声协方差R_k引入新的抑制因子：

E＝(A-2B)(A-2B)^T

其中，λ为自适应抑制因子，能够起到抑制观测噪声v_k对滤波结果产生的影响的作用。引入自适应抑制因子的计算模型可以表述为：

更新过程如下：

其中，

为移动终端当前位置的预测值，F为状态转移矩阵，BU_k-1为系统控制项，s_k为预测噪声，其协方差矩阵为Q，Y_k为当前时刻移动终端位置的测量值。

至此，利用通过无线定位基站计算得到的移动终端位置测量值，结合上述引入抑制因子进行误差控制的卡尔曼滤波自适应定位算法，即可得到移动终端最终的位置坐标。

依据表1中所示的仿真参数设置仿真环境，可以得到如图1、图2和图3所示的仿真结果。

表1系统仿真参数设置

移动终端初始位置	(50，0)
		移动终端行驶速度	(20，0)
采样周期	0.6s
		基准站位置	(0，0)
定位基站1位置	(1000，-500)
		定位基站2位置	(500，600)
定位基站3位置	(1500，500)
		采样次数	200

图1反映了加入自适应抑制因子前的自适应卡尔曼滤波算法的计算效果，图2反映了本发明提供的基于误差控制的卡尔曼滤波自适应滤波算法的计算效果，图3为本发明实施例提供的基于误差控制的卡尔曼滤波自适应滤波算法与加入自适应抑制因子前的自适应卡尔曼滤波算法计算结果的均方根误差对比情况。

从图1所示仿真轨迹可以看出，加入自适应抑制因子前，在初始滤波计算的过程中，定位最终结果波动较大，出现了数据发散的问题，随着计算次数的增加，其定位结果才逐渐趋于稳定，维持在一个较好的水平。而采用了本发明提出的基于误差控制的卡尔曼滤波自适应滤波算法，即便是在初始计算的时候，也并未出现这样的问题，而是保证了系统状态在整个计算过程当中，其状态估计结果始终保持在一定的误差范围之内，其状态估计结果的准确程度较之传统的自适应卡尔曼滤波算法，具有更好的稳定性。

图3反映的是本发明提供的基于误差控制的卡尔曼滤波自适应滤波算法与加入自适应抑制因子前的自适应卡尔曼滤波算法计算结果的均方根误差对比情况。通过比较系统状态估计结果的均方根误差，可以看到更为明显的对比结果。采用加入自适应抑制因子前的自适应卡尔曼滤波算法进行估计，在初始计算的过程中出现了估计结果发散的现象，其均方根误差最大时接近70m，随着计算次数的增加，其误差逐渐减小，最后稳定在20m以内。而本发明提供的基于误差控制的卡尔曼滤波自适应滤波算法的估计结果较为稳定，从初始滤波计算开始，其均方根误差始终稳定在30m以内，随着计算次数的增加，其状态估计结果的均方根误差也基本稳定在20m以内。由此可以证明加入自适应抑制因子的改进算法有效，其滤波估计结果较改进前更为稳定。

此外，需要说明的是，本领域内的技术人员应明白，本发明实施例的实施例可提供为方法、装置、或计算机程序产品。因此，本发明实施例可采用完全硬件实施例、完全软件实施例、或结合软件和硬件方面的实施例的形式。而且，本发明实施例可采用在一个或多个其中包含有计算机可用程序代码的计算机可用存储介质(包括但不限于磁盘存储器、CD-ROM、光学存储器等)上实施的计算机程序产品的形式。

本发明实施例是参照根据本发明实施例的方法、终端设备(系统)、和计算机程序产品的流程图和/或方框图来描述的。应理解可由计算机程序指令实现流程图和/或方框图中的每一流程和/或方框、以及流程图和/或方框图中的流程和/或方框的结合。可提供这些计算机程序指令到通用计算机、嵌入式处理机或其他可编程数据处理终端设备的处理器以产生一个机器，使得通过计算机或其他可编程数据处理终端设备的处理器执行的指令产生用于实现在流程图一个流程或多个流程和/或方框图一个方框或多个方框中指定的功能的装置。

这些计算机程序指令也可存储在能引导计算机或其他可编程数据处理终端设备以特定方式工作的计算机可读存储器中，使得存储在该计算机可读存储器中的指令产生包括指令装置的制造品，该指令装置实现在流程图一个流程或多个流程和/或方框图一个方框或多个方框中指定的功能。这些计算机程序指令也可装载到计算机或其他可编程数据处理终端设备上，使得在计算机或其他可编程终端设备上执行一系列操作步骤以产生计算机实现的处理，从而在计算机或其他可编程终端设备上执行的指令提供用于实现在流程图一个流程或多个流程和/或方框图一个方框或多个方框中指定的功能的步骤。

尽管已描述了本发明的优选实施例，但本领域内的技术人员一旦得知了基本创造性概念，则可对这些实施例做出另外的变更和修改。所以，所附权利要求意欲解释为包括优选实施例以及落入本发明实施例范围的所有变更和修改。

还需要说明的是，在本文中，术语“包括”、“包含”或者其任何其他变体意在涵盖非排他性的包含，从而使得包括一系列要素的过程、方法、物品或者终端设备不仅包括那些要素，而且还包括没有明确列出的其他要素，或者是还包括为这种过程、方法、物品或者终端设备所固有的要素。在没有更多限制的情况下，由语句“包括一个……”限定的要素，并不排除在包括所述要素的过程、方法、物品或者终端设备中还存在另外的相同要素。

以上所述仅为本发明的较佳实施例，并不用以限制本发明，凡在本发明的精神和原则之内，所作的任何修改、等同替换、改进等，均应包含在本发明的保护范围之内。

Claims (4)

1.一种基于误差控制的卡尔曼滤波自适应滤波算法，其特征在于，包括以下步骤：

根据观测误差的协方差矩阵设计自适应抑制因子，并利用所述自适应抑制因子构建观测误差协方差矩阵的自适应修正模型；

利用所述自适应抑制因子及所述自适应修正模型对滤波结果进行误差控制，以系统状态实际测量值为基础，结合基于误差控制的卡尔曼自适应滤波算法，对系统状态进行滤波估计。

2.根据权利要求1所述的基于误差控制的卡尔曼滤波自适应滤波算法，其特征在于，采用Sage-Husa卡尔曼自适应滤波算法对系统状态进行滤波估计，计算模型为：

预测过程如下：

引入调节更新参量：

更新过程如下：

其中，P_k为估计值与真实值之间的误差协方差矩阵，H为观测矩阵，d_k为引入的调节更新参量，b为遗忘因子，取值范围在0.95-0.99之间，K为卡尔曼增益。

3.根据权利要求2所述的基于误差控制的卡尔曼滤波自适应滤波算法，其特征在于，设计自适应抑制因子的步骤包括：

R_k＝(1-d_k)R_k-1+A+B

其中R_k的两个余项A和B分别为：

为观测噪声协方差R_k引入新的抑制因子：

E＝(A-2B)(A-2B)^T

其中，λ为自适应抑制因子，R_k为当前时刻观测噪声协方差矩阵，A和B为观测噪声协方差矩阵的两个余项，v_k为观测噪声，P_k为先验估计协方差，H为观测矩阵，d_k为引入的调节更新参量，b为遗忘因子，取值范围在0.95-0.99之间，K为卡尔曼增益，η为余项的偏差。

4.根据权利要求3所述的基于误差控制的卡尔曼滤波自适应滤波算法，其特征在于，引入自适应抑制因子的卡尔曼自适应滤波算法的计算模型为：

更新过程如下：

其中，

为系统状态的预测值，F为状态转移矩阵，BU_k-1为系统控制项，s_k为预测噪声，其协方差矩阵为Q，Y_k为系统状态的测量值。

Images