[发明专利]一种改进的强跟踪滤波方法

权利要求书

1.一种改进的强跟踪滤波方法，其特征在于包括下述步骤：

步骤1：建立状态和量测模型，所述状态和量测模型包括带有噪声相关和一步随机时滞量测的非线性系统模型、正定矩阵构造的伪过程噪声模型、带有噪声相关和一步随机时滞量测的非线性系统的恒等状态空间模型：

其中，所述建立带有噪声相关和一步随机时滞量测的非线性系统模型过程如下：

步骤1.1：考虑离散时间非线性随机系统

其中{x_k；k≥0}表示n×1维状态向量,{z_k；k≥1}表示m×1维实际量测向量，{y_k；k≥1}表示m×1维可用量测向量，f_k(·)和h_k(·)是能够无限连续可微分的非线性函数，{w_k；k≥0}和{v_k；k≥1}是满足协方差矩阵和的相关零均值高斯白噪声序列，δ_kl表示Kronecker函数，初始状态x₀独立于{w_k；k≥0}和{v_k；k≥1}，表示高斯随机向量满足和{γ_k；k＞1}表示可以取值0-1互不相关伯努利随机变量序列

其中，p_k表示k时刻时滞概率，

所述定矩阵构造的伪过程噪声模型建立过程如下：

步骤1.2：为了解耦过程噪声和量测噪声的相关性，引入了一个正定矩阵，

I表示单位矩阵，R_k和S_k分别表示量测噪声v_k的协方差和过程噪声w_k的互协方差，因此，我们得到

其中是伪过程噪声满足和伪过程噪声和量测噪声是互不相关的，因为

所述带有噪声相关和一步随机时滞量测的非线性系统的恒等状态空间模型建立过程如下：

步骤1.3：将等式带入(1)中的表达式x_k+1,得到

定义F_k(x_k)＝f_k(x_k)+J_kv_k,然后，(1)中的离散非线性动态系统被转换成以下形式

z_k＝h_k(x_k)+v_k,k≥1, (4)

其中x₀,{v_k；k≥1}和{γ_k；k＞1}都是相互独立的，

步骤2：由于恒等状态空间模型中所示的非线性系统模型满足过程噪声和量测噪声是互不相关的，因此给出了DF的框架，其次，在此框架的基础上，提出了一种新的基于一阶线性逼近的EKF算法，所述DF的框架的建立包括状态预测和状态更新，

所述DF的框架的建立过程如下：

步骤2.1：继续考虑(3)-(5)中所示的非线性系统模型，将(4)代入(5)中,我们得到

y_k+1＝(1-γ_k+1)[h_k+1(x_k+1)+v_k+1]+γ_k+1[h_k(x_k)+v_k]. (6)

根据等式(6)，我们推导DP的框架时需要获得MMSE的前两个时刻p(x_k+1|Y_k+1)和p(v_k+1|Y_k+1)，因此,需要定义一个扩展状态向量，如下所示：

其中MMSE的的前两个时刻，如下所示：

在(11)中,和分别是扩展状态k+1时刻的状态和量测噪声的滤波估计和协方差，是k+1时刻的状态噪声和量测噪声的互协方差，v_k+1和y_k以及x_k+1相互独立，扩展状态和协方差分别是

定义均值、协方差和互协方差为

其中是(5)中的是可用量测集，

所述状态预测过程如下：

步骤2.1.1：将(3)代入(10)中，和P_k+1|k的表示为

在已知考虑是独立于v_k和Y_k的，我们得到

将(13)-(14)代入到(9)，得到扩展状态的预测估计

所述状态更新过程如下：

步骤2.1.2：已知和v_k，γ_k与Y_k互相独立，我们得到

其中K_k是扩展状态的扩展矩阵，且

所述一种新的基于一阶线性逼近的EKF算法实现过程如下所示：

步骤2.2：在(11)-(12)和(15)-(20)中，实现DF的关键是计算(13)-(14)和(21)-(26)中的高斯加权积分，由于f_k(·)和h_k(·)的非线性，上述积分的计算过程是无法完成的，因此，需要数值近似估计，例如一阶线性化估计，在这里，我们使用带有基于一阶线性化的一步随机时滞量测的EKF来实现DF，

给定滤波估计将f_k(x_k)和h_k(x_k)线性化，得到

其中等式(13)-(14)的近似如下所示：

进一步，将(30)-(31)代入(9)得到预测估计

给定预测估计关于线性化h_k+1(x_k+1)得到

其中(21)-(26)近似如下：

将(33)-(38)代入(15)-(20)，可以计算增强状态的滤波估计

步骤3：标准强跟踪滤波(Strong Tracking Filter，STF)特别适用于这些情况下的非线性状态估计，即模型不确定性，噪声相关和随机时滞量测，然而，上述STF不能直接应用于(1)中所示的非线性系统中，这是因为基于正交性准则选择的残差对是根据没有随机时滞量测结果来计算的，因此，给出了在(1)中的非线性系统中应用的EOP，所述基于EOP的STF模型的建立包括渐消因子的引入与计算和随机时滞量测和噪声相关的STF模型的建立，具体步骤如下所示：

等式(39)是所提出的EKF的性能指标，等式(40)意味着根据(6)和(18)计算的是相互正交的任意选择的残差对，

所述渐消因子的引入与计算过程如下：

步骤3.1：当系统模型准确时，基于给定的可用量测值所提出的EKF提供了对增广状态的次优估计，然而，当模型不确定时，EKF的估计性能将会很差甚至发散，基本问题是，(18)中所示的增益矩阵不能适应可用测量和预测测量之间残差的变化，为了克服这个问题并使得所提出的EKF具有STF的优良特性，自然的想法是将EOP与所提出的EKF结合以将次优退化因子代入到增广状态的滤波估计中来导出STF/RDMCN，修改后的滤波估计如下：

其中λ_k+1(λ_k+1≥1)表示次优退化因子，

将(41)代入(31)中，我们得知状态的预测估计P_k+1|k也同样被相同的次优退化因子改变了，等式(34)，(36)-(38)和(17)，我们发现STF/RDMCN可以利用时变的渐消因子来破坏的历史状态的影响，并且实时调整增广状态的增益矩阵，以提高滤波的跟踪性能，

然后，下一步的工作是根据EOP确定次优衰落因子λ_k+1，

根据(6)，(18)，(29)，(32)，(33)和(35)，我们得到

其中

用(28)减去(30)得到

将(43)代入(42)得到

使用相似的推导过程，我们得到

将(45)代入(40)得到

考虑x₀，{v_k；k≥1}，{γ_k；k＞1}和Y_k是相互独立的并结合等式(2)，等式(46)被简化为以下形式，即

其中

根据(15)，(8)和(9)，我们得到

将(49)代入(47)中的和我们得到

基于(43)，(45)并使用(46)中的相似简化过程，和简化成

其中

将(51)代入(47)，重新整理(47)得到

其中

根据(47)和(53)，我们可以通过使用迭代方法来获得即

其中

当i＝1，得到公式(55)的形式，

当j＝1时利用(49)，(10)中以及的表达式，我们得到公式(59)，即

其中是残差的协方差，

将(17)代入(60)，得到

根据(61)，如果选择(41)中的适当的渐消因子λ_k+1能保证

然后满足EOP，将(19)，(34)和(36)代入(62)，重新整理(62)得到

将(31)和(41)代入(63)，整理(63)得到

为了获取渐消因子λ_k+1，公式(64)两边引入跟踪后得到：

定义：

因此，等式(67)简化为

tr[λ_k+1M_k+1]＝tr[N_k+1]. (68)

然后，渐消因子λ_k+1表达式如下

尽管如此，公式(67)中，残差的协方差未知，这可以通过以下方法决定，

其中ρ(0＜ρ≤1)是一个遗忘因子，通常选择ρ＝0.95，当λ_k+1≥1，次优退化因子λ_k+1才能起作用，因此λ_k+1可以通过以下方式计算：

所述随机时滞量测和噪声相关的STF模型的建立过程如下：

步骤3.2现在，我们用一阶线性化方法去估计(13)-(14)和(21)-(26)中的积分部分，得到一个新的STF，我们将得到的STF/RDMCN应用到非线性系统模型中，过程如下：

1)初始化(k＝0)

2)当k＝1

第1步：渐消因子的引入与计算

M₁和N₁计算如下

其中V₁⁰可以通过(70)计算，将M₁和N₁代入(71)得到λ₁，然后，将λ₁代入(41)得到

第2步：状态预测

P_1|0可以通过计算得到，预测估计可以通过将和P_1|0代入(9)中计算得到，

第3步：状态更新

和计算如下：

滤波估计可以通过将和(74)代入(15)-(17)计算得到，

3)当k＞1

第1步：渐消因子的引入与计算

假定在时刻k，滤波估计和残差协方差均已知，在k+1时刻，M_k+1和N_k+1能通过(30)，(18)，(70)，(66)和(67)分别计算得到，将M_k+1和N_k+1代入(71)得到λ_k+1，然后，引入λ_k+1到(41)得到第2步：状态预测

P_k+1|k通过下式计算得到：

预测估计可以通过将和P_k+1|k代入到(9)中计算得到，

第3步：状态更新

和可以通过(34)和(37)计算得到，和可以通过下式计算得到

一旦获得新的量测值y_k+1，将和(76)代入(15)-(20)计算在k+1时刻的滤波估计