基于时变自调整的滚动时域估计到达代价计算方法与流程

本发明属于飞行器姿态估计滤波技术领域，更具体地，本发明涉及一种滚动时域估计滤波的时变自调整到达代价计算方法，可用于强干扰下的飞行器姿态估计。

背景技术

在过去的几十年里，基于优化的滚动时域估计(movinghorizonestimation,mhe)在非线性约束问题中应用非常广泛。和全信息估计相比，mhe通过仅考虑过去有限量测值来减小计算负担。过去历史信息对当前状态的影响即为到达代价。对于线性无约束姿态估计系统，标准卡尔曼滤波方差更新公式可以用来计算到达代价，但是对于非线性有约束姿态估计系统，到达代价的通用解析表达式不再适用。

对于非线性系统的到达代价的计算方法，目前主要研究方向都是针对ekf需要泰勒级数展开精度不高以及雅可比矩阵计算较为复杂的问题进行讨论。然而，以上这些文献都没有考虑算法的鲁棒性。事实上，由于传感器实际工作状态与线下状态有很大差别，极端的温度、压力和电磁环境使实际系统的参数发生变动，造成原来比较准确的模型也与实际系统不再匹配。同时，由于受到外界电磁信号的干扰，所建模型的噪声统计特性与实际噪声的统计特性可能有较大的差异。所有这些因素，包括对初始状态的不准确估计，都会影响滤波器的精度。因而，寻找一种鲁棒性强的到达代价计算方法在滤波估计系统中至关重要。

带有时变自调整因子的滤波器可以克服这些问题。它是一种效果良好的强跟踪滤波器，可以在线调整滤波增益，从而达到算法自适应的目的。不仅具有极强的模型失配鲁棒性，且具有概念清晰、计算简单的优点。将时变自调整因子滤波器的自适应性与ukf的非线性高逼近性结合起来，使得本方法具有较强的自适应能力和鲁棒性。

技术实现要素：

本发明的目的在于突破传统滤波算法在抗干扰能力的不足，提高系统鲁棒性，提高系统自适应能力，提出一种基于强跟踪的滚动时域估计到达代价计算方法。其中解决的主要问题包括：

(1)对于非线性系统的到达代价的计算方法，目前主要研究方向都是针对ekf需要泰勒级数展开精度不高以及雅可比矩阵计算较为复杂的问题进行讨论。没有考虑算法的鲁棒性。

(2)由于传感器实际工作状态与线下状态有很大差别，极端的温度、压力和电磁环境使实际系统的参数发生变动，造成原来比较准确的模型也与实际系统不再匹配。

(3)由于较容易受到外界电磁信号的干扰，所建模型的噪声统计特性与实际噪声的统计特性可能有较大的差异。

(4)对初始状态的不准确估计，会影响滤波器的精度。

本发明所述的基于时变自调整的滚动时域估计到达代价计算方法，其特征在于包括以下技术措施：

步骤一：利用状态方程、观测方程的线性化矩阵，加入渐消因子，计算时变自调整因子，以实现对到达代价的时变自调整；

步骤二：在得到时变自调整因子的基础上，利用状态方程及观测方程，计算带有时变自调整因子的系统状态及方差的粗略估计值；

步骤三：在得到状态及方差的粗略估计值，利用带有时变自调整因子的协方差计算式，计算下一时刻状态及方差粗略估计值精调整值；

步骤四：将上步中输出的下一时刻状态及方差粗略估计值精调整值带入mhe系统计算到达代价。

对比现有技术，本技术方案所述的基于时变自调整因子的滚动时域估计到达代价计算方法，有益效果在于：

(1)本方法通过时变自调整因子无迹卡尔曼滤波计算滚动时域估计问题的到达代价，充分考虑了算法的鲁棒性；

(2)当传感器实际工作状态与线下差别较大，系统参数发生变化时，计算误差较小；

(3)当受到外界电磁信号干扰时，具有较强的抵抗能力，所建模型的噪声统计特性与实际特性差异大时能够自适应修正；

(4)可以修正对初始状态的不准确估计。

附图说明

附图1是基于时变自调整因子的滚动时域估计到达代价计算方法实施流程图。

附图2是滚动时域估计滤波方法基本原理图。

附图3是滚动时域估计滤波计算方法流程图。

具体实施方式

为了解决滚动时域估计方法鲁棒性不强，抗干扰能力较差的问题，提出了基于时变自调整因子的到达代价计算方法。首先，设计了时变自调整次优调节因子的计算方法，然后将该方法带入到ukf滤波算法中，计算滚动时域估计“窗口”外的状态估计值以及方差，最后将得到的估计值和方差带入到滚动时域估计系统中计算到达代价。

以下结合说明书附图1对本发明做进一步的详细描述。参照说明书附图1，本发明的处理流程分以下步骤：

(1)系统开始状态的赋值

(2)针对n维状态向量，根据中心扩展采样法选取代表点并计算加权数

假设状态向量x为n维，选取2n+1个采样点xk,p0，p0＝0,...,2n。中心扩展采样法规则如下：

加权数计算方法如下：

其中，ρ0＝τ²(n+κ)-n，τ通常取一个很小的数，对于高斯先验分布的情形μ＝2最优，κ通常取0。

(3)时变自调整次优调节因子的计算

时变自调整次优调节滤波器的一个充分条件是在线选择一个适当的时变增益矩阵使得残差正交。当滤波器状态估值偏离系统的状态时，结果会在残差序列的均值和幅值上表现出来。这时，可以通过增加时变次优调节因子λ来实时调整状态预报误差协方差，强迫残差序列仍然保持正交，从而达到调整滤波增益矩阵形成强跟踪滤波器的目的。时变滤波增益矩阵中的次优调节因子：

其中

λ0＝trnk/trmk

fk是状态方程的线性化矩阵，在算法中形式是hk是观测方程的线性化矩阵，在算法中的形式为tr(·)为矩阵的迹；

0≤ρ≤1为渐消因子，通常取值0.95。引入渐消因子的目的是为了进一步提高滤波器的快速跟踪能力。滤波的实际误差不应小于理论误差，且不同的应用场合对系统的跟踪性能和滤波性能的侧重不同。φ为减小系数，一般取经验值。

(4)下一时刻状态及方差的粗略估计值的计算

χp0,k|k-1＝f(xp0,k-1)，p0＝0,...,2n

γp0,k|k-1＝h(χp0,k|k-1)

(5)下一时刻状态及方差的粗略估计值精调整

(6)滚动时域估计算法到达代价的计算

滚动时域估计算法的基本思想为，只考虑当前时刻以前固定数量的数据，同时将历史数据对估计的影响用一种方法来近似描述，则每次参与优化的数据量不变，这种方法实现起来就像一个滚动的观测“窗口”。这个“窗”包含过去有限的量测值，“窗”的最后一个输出为当前状态估计，附图2为滚动时域估计滤波方法基本原理图。

设窗口长度为l(i＝1,2,..,l)，其中窗口最后的一个点(i＝l)代表现在时刻k。滚动时域估计问题可表示为

s.t.

xi+1＝f(xi,ui)+wi,fori＝1,…,l-1(3b)

yi＝h(xi)+vi,fori＝2,…,l(3c)

li(xi)＝0,fori＝1,…,l(3d)

其中xi是待估计的状态。式(3b)描述了在离散情况下的状态递推过程。在该递推方程中ui是已知输入，它的过程噪声为wi，过程噪声方差为qi。方程(3c)为量测方程，yi为量测值，vi、ri分别为量测噪声和量测噪声方差。在时域内的第一个时刻点为方差为p1⁰。(3a)第三项θk为代价函数，它描述了到达代价。值得注意的是i＝1时刻的量测值并没有包含在代价函数中。假定从量测中得到的所有信息已经包含在先验估计中。等式约束由式(3d)给出。滚动时域估计问题的输出为xl：时域“窗”中最后一个时刻的估计状态向量。

将式(1)、(2)带入到滚动时域估计系统(3e)中就可以得到到达代价θk。附图3是滚动时域估计滤波计算方法流程图。