CN106933106A - 一种基于模糊控制多模型算法的目标跟踪方法 - Google Patents

Abstract

本发明提供的是一种基于模糊控制多模型算法的目标跟踪方法。引入机动目标的动力学建模方法，对匀速和蛇形机动模型进行建模；对匀速和蛇形机动模型条件重新初始化，计算滤波器的混合输入；利用滤波器的混合输入，对匀速和蛇形机动模型采用无迹卡尔曼滤波算法进行并行滤波计算目标状态的初始值，并进行目标状态估计；采用改进的误差协方差统计估值器，递推估计系统误差协方差统计特性；分别计算k时刻匀速和蛇形机动模型的概率，并利用模糊逻辑算法进行模型概率更新；利用获得的当前目标状态估计和模型概率计算总体状态输出。本发明估计误差的收敛速度快，鲁棒性强，能够降低纯方位目标跟踪的误差，实现对机动目标末端“蛇形”运动模型的稳定跟踪。

Description

一种基于模糊控制多模型算法的目标跟踪方法

技术领域

本发明涉及的是一种模式识别方法，尤其是一种机动目标跟踪方法。

背景技术

“蛇形”机动是机动目标的典型运动方式之一，其运动速度高、突防能力强。目前对机动目标的研究主要有三个方面：①综述性的介绍。②运动轨迹的模拟。③拦截目标与机动目标的对抗过程的模拟。可见在目标跟踪领域中对机动目标的研究还是有限的，尤其是将运动轨迹与跟踪相结合的研究少之又少。2012年，张亮亮提出新的“S-蛇形”机动模型，它是在原始机动转弯模型的基础上进行三维补维，并对状态转移矩阵进行修正，得到“蛇形”机动的模型。2014年，Xu Tian Ye的模拟机动目标蛇形轨迹是对其加速度进行变化，得到类似的“蛇形”机动轨迹。这两种方法由于忽略了重力以及过载的影响，都不符合机动目标的运动规律。

在机动目标跟踪中普遍采用交互式多模型算法，该算法的模型转换概率矩阵是先验矩阵，准确、快速的对其进行调整将会更有效地实现对机动目标的跟踪。为此，Wang X.F，Peng DL提出一种基于模糊逻辑的交互式多模型自适应无迹卡尔曼滤波算法(FLIMM-AUKF)(Fuzzy-logic adaptive variable structure multiple-model algorithm fortracking a high maneuvering target[J].Journal of the Franklin Institute-Engineering and Applied Mathematics,2014,351(7):3837-3846.)，利用模糊控制方法实现实时调整交互式多模型算法中的转换概率矩阵。在滤波算法中对Sage-Husa噪声估值器进行了改进，占荣辉给出了常规的推导过程，随后，石勇，李昱辰(自适应UKF算法在目标跟踪中的应用[J].自动化学报,2011,37(6):756-759.)将Sage-Husa噪声估计与无迹卡尔曼滤波、粒子滤波等结合，提出了自适应算法。但不适用于强机动目标，而且会出现粒子退化现象。

发明内容

本发明的目的在于提供一种估计误差的收敛速度快，鲁棒性强，并且能够有效降低纯方位目标跟踪的误差，实现对机动目标末端“蛇形”运动模型的稳定跟踪的基于模糊控制多模型算法的目标跟踪方法。

本发明的目的是这样实现的：

步骤一：引入机动目标的动力学建模方法，对匀速模型和蛇形机动模型进行建模；

步骤二：对匀速模型和蛇形机动模型条件重新初始化，计算滤波器的混合输入；

步骤三：利用滤波器的混合输入，对匀速模型和蛇形机动模型采用无迹卡尔曼滤波算法进行并行滤波计算目标状态的初始值，并进行目标状态估计；

步骤四：采用改进的误差协方差统计估值器，递推估计系统误差协方差统计特性；

步骤五：分别计算k时刻匀速模型和蛇形机动模型的概率，并利用模糊逻辑算法进行模型概率更新；

步骤六：当前时刻状态估计融合，利用获得的当前目标状态估计和模型概率计算总体状态输出。

本发明还可以包括：

1、所述的引入机动目标的动力学建模方法，具体包括：

(1)对目标做蛇形机动进行动力学建模推导，机动加速度按正弦规律变化，目标坐标系中三个方向上的重力加速度和目标过载为：

其中：和为重力加速度在目标坐标系中三个坐标轴方向的分量；和为目标过载在目标坐标系中三个坐标轴方向的分量；g为重力加速度；θ为目标的运动倾角；a₀为机动加速度幅度；ω为机动频率；

(2)采用龙格库塔法计算目标在惯性坐标系中的位置分量x、y和z，再经过坐标转换，将惯性坐标系转换到目标坐标系，得质心运动方程表示为：

其中：和为目标在惯性坐标系中的速度分量；和为目标在惯性坐标系中的加速度分量；v为目标的运动速率；为目标的运动速率的导数；ψ_v为目标的运动偏角；为目标的运动偏角的导数，为目标的运动倾角的导数。2、所述采用改进的误差协方差统计估值器，递推估计系统误差协方差统计特性，具体包括：

(1)定义非线性离散时间系统：

其中：X(k)为k时刻n维被估计状态向量；Z(k)为k时刻m维观测向量；f[·]为n维可微向量函数；G(k-1)为k-1时刻n×r维过程噪声转移矩阵；W(k-1)为k-1时刻r维过程噪声；h[·]为m维可微向量函数；V(k)为k时刻m维观测噪声；

(2)计算误差协方差：

设误差协方差是未知的定常向量或矩阵，自适应滤波问题就是基于观测求误差协方差和状态X(k)；

(3)当误差协方差P_x未知时，连同状态X(0),…,X(k)的极大后验即MAP估计k时刻平滑估值x(j/k)用极大化条件概率密度求得：

J^*＝p[X(k),P_x|Z(k)]

式中，X(k)＝{x(0),x(1),…,x(k)}；Z(k)＝{z(1),z(2),…,z(k)}；p[·]表示变量概率；

由Bayes公式

(4)p[Z(k)]与最优化无关，转化为求如下无条件概率密度的极大值：

J＝p[X(k),P_x,Z(k)]＝p[Z(k)|X(k),P_x]·p[X(k)|P_x]·p[P_x]；

(5)设P_x服从均匀分布，由正态假设易知

有

其中，x(j)为平滑估值；x(j-1)为预平滑估值；x(0)为初始状态；P₀为初始误差协方差；c,d₁,d₂,const均为常数；

J与lnJ有相同极点，设x(j/k)已知，则令

得误差协方差统计的MAP估值器为

(6)次优MAP估值器

以滤波估值x(j/j)或预报估值x(j/j-1)近似代替计算复杂的平滑估值x(j/k)，可得次优MAP估值器。

(7)次优无偏极大后验估值器

式中

所以其中，P_x(j/j-1)为X的预滤波估值协方差，P_x(j/j)为X的滤波估值协方差；

(8)引出递推无偏MAP估值器为

本发明提出了基于误差协方差估计的无迹卡尔曼滤波算法，通过对“蛇形”模型的跟踪仿真验证了该算法收敛速度快、误差小，具有实用价值。

本发明的核心技术内容在于引入动力学建模方法，把目标视为可控质点，建立了机动目标末端“蛇形”机动模型；同时根据Sage-Husa噪声统计估值器的原理，利用模糊控制方法实现实时调整交互式多模型算法中的转换概率矩阵，提出一种新的基于模糊控制的交互式多模型算法的目标跟踪方法。

本发明提供的方法根据Sage-Husa噪声统计估值器的原理，改进跟踪算法的调整模型概率速度，使得其估计误差的收敛速度比交互式多模型算法更快，鲁棒性更强，并且能够有效降低纯方位目标跟踪的误差，实现对机动目标末端“蛇形”运动模型的稳定跟踪。

附图说明

图1是本发明的流程图。

图2(a)是本发明模糊控制概率更新模块的输入变量I₁的隶属度函数示意图，图2(b)是本发明模糊控制概率更新模块的输入变量I₂的隶属度函数示意图，图2(c)是本发明模糊控制概率更新模块的输出变量u的隶属度函数示意图。

图3是本发明目标运动动力学模型三维视图；

图4(a)是本发明仿真试验中采用IMM-UKF、自适应IMM-UKF与本发明算法跟踪机动目标位置x方向估计误差，图4(b)是本发明仿真试验中采用IMM-UKF、自适应IMM-UKF与本发明算法跟踪机动目标位置y方向估计误差，图4(c)是本发明仿真试验中采用IMM-UKF、自适应IMM-UKF与本发明算法跟踪机动目标位置z方向估计误差。

图5(a)是本发明仿真试验中采用IMM-UKF、自适应IMM-UKF与本发明算法跟踪机动目标速度x方向估计误差，图5(b)是本发明仿真试验中采用IMM-UKF、自适应IMM-UKF与本发明算法跟踪机动目标速度y方向估计误差，图5(c)是本发明仿真试验中采用IMM-UKF、自适应IMM-UKF与本发明算法跟踪机动目标速度z方向估计误差。

图6(a)是本发明仿真试验中采用IMM-UKF、自适应IMM-UKF与本发明算法跟踪机动目标加速度x方向估计误差，图6(b)是本发明仿真试验中采用IMM-UKF、自适应IMM-UKF与本发明算法跟踪机动目标加速度y方向估计误差，图6(c)是本发明仿真试验中采用IMM-UKF、自适应IMM-UKF与本发明算法跟踪机动目标加速度z方向估计误差。

具体实施方式

下面结合具体实施例对本发明进行详细说明。

结合图1，本发明实现步骤如下：

步骤一：引入机动目标的动力学建模方法，对匀速模型和“蛇形”机动模型进行建模；

(1.1)对目标做“蛇形”机动进行动力学建模推导，机动加速度按正弦规律变化，可知目标坐标系中三个方向上的重力加速度和目标过载如式(1)所示：

其中，和为重力加速度在目标坐标系中三个坐标轴方向的分量；和为目标过载在目标坐标系中三个坐标轴方向的分量；g为重力加速度；θ为目标的运动倾角；a₀为机动加速度幅度；ω为机动频率。

(1.2)采用龙格库塔法计算目标在惯性坐标系中的位置分量x、y和z，再经过坐标转换，将惯性坐标系转换到目标坐标系，可得其质心运动方程如式(2)所示：

其中，和为目标在惯性坐标系中的速度分量；和为目标在惯性坐标系中的加速度分量；v为目标的运动速率；为目标的运动速率的导数；ψ_v为目标的运动偏角；为目标的运动偏角的导数，为目标的运动倾角的导数。(1.3)将上述动力学“蛇形”质心运动方程及位置分量x、y和z，代入传统“蛇形”机动模型中，得到动力学方式建模的“蛇形”机动模型：

X(k+1)＝FX(k)+GW(k) (3)

其中，F，G分别为过程矩阵和过程噪声传递矩阵；W(k)＝[w₁(k) w₂(k)]^T，w₁(k)，w₂(k)为均值为0，方差为σ²的高斯白噪声系统分量；k为离散时刻；ω为机动频率；T为采样时间。

(1.4)按照同样的推导，可得匀速模型的动力学模型。描述机动目标运动方程的第j(j＝1,2)个数学模型表示为：

其中，F_j(k)是第j个模型的过程矩阵；G_j(k)是第j个模型的过程噪声传递矩阵；Z(k)是观测矩阵；H_j(k)是第j个模型的观测矩阵；过程噪声w_j(k)和观测噪声v_j(k)是相互独立的零均值高斯白噪声序列。

步骤二：对匀速模型和“蛇形”机动模型条件重新初始化，计算滤波器的混合输入；

已知k-1时刻两模型的滤波状态估计为估计误差协方差矩阵为P_j(k-1/k-1)，各模型之间的转移概率服从马尔科夫过程，其概率转移矩阵为k-1时刻匹配的模型为i，k时刻匹配的模型为j。

(2.1)输入交互运算。输入交互运算是在给定模型上一时刻的状态、协方差估计值并获得新的量测Z(k)之后对模型进行重新初始化运算。计算混合概率：

其中，p_ij表示从模型i到模型j的转移概率，且i,j＝1,2；μ_j(k-1)表示各模型的概率；

(2.2)滤波器重初始化状态与协方差阵按混合估计分别为：

步骤三：利用滤波器的混合输入，对匀速模型和“蛇形”机动模型采用无迹卡尔曼滤波算法进行并行滤波计算目标状态的初始值，并进行目标状态估计；

(3.1)状态变量x为n维随机变量，其均值为协方差为随机变量P_x，则可以构造随机变量2n+1个加权采样点来近似这个随机变量的分布，设定状态与协方差阵初始值：

其中，X(0)为0时刻的状态变量，E[·]为对变量求均值。

(3.2)预测更新。当采样时刻k＞1时，构造2n+1个Sigma点：

其中，为k-1时刻模型的滤波状态估计；为k-1时刻状态变量X的协方差阵估计；λ＝α²(n+κ)-n,0≤α≤1，对于状态变量x高斯分布的情况，若n＝1，则κ＝2，否则取κ＝3-n，调节α可以减小预测误差；W_i ^(m)＝W_i ^(c)为与第i个点x_i相对应的权值。

计算预测Sigma点：

其中，f[·]为n维可微向量函数。

计算预测Sigma点的均值和方差：

其中；Q(k)为噪声w(k)的协方差矩阵。

(3.3)输出测量预测残差、协方差阵和似然函数：

其中，H_j(k)是第j个模型的观测矩阵；R_j(k)为v_j(k)的协方差矩阵。

(3.4)测量更新。根据状态条件预测值，计算滤波增益矩阵，状态估计方程，状态误差协方差矩阵：

其中， 为的均值，且h[·]为m维可微向量函数；R(k)为v(k)的协方差矩阵；K^T为K的转置。

步骤四：采用改进的误差协方差统计估值器，递推估计系统误差协方差统计特性。

(4.1)定义非线性离散时间系统：

式中，X(k)为k时刻n维被估计状态向量；Z(k)为k时刻m维观测向量；f[·]为n维可微向量函数；G(k-1)为k-1时刻n×r维过程噪声转移矩阵；W(k-1)为k-1时刻r维过程噪声；h[·]为m维可微向量函数；V(k)为k时刻m维观测噪声。

(4.2)计算误差协方差：

误差协方差是未知的定常向量或矩阵，自适应滤波问题就是基于观测求误差协方差和状态X(k)。

(4.3)当P_x未知时，连同状态X(0),…,X(k)的极大后验(MAP)估计k时刻平滑估值x(j/k)可以用极大化条件概率密度求得：

J^*＝p[X(k),P_x|Z(k)] (25)

式中，X(k)＝{x(0),x(1),…,x(k)}；Z(k)＝{z(1),z(2),…,z(k)}；p[·]表示变量概率。

由Bayes公式

(4.4)p[Z(k)]与最优化无关，因而问题转化为求如下无条件概率密度的极大值：

J＝p[X(k),P_x,Z(k)]＝p[Z(k)|X(k),P_x]·p[X(k)|P_x]·p[P_x] (27)

(4.5)假设P_x服从均匀分布，由正态假设易知

类似有

其中，x(j)为平滑估值；x(j-1)为预平滑估值；x(0)为初始状态；P₀为初始误差协方差；c,d₁,d₂,const均为常数。

J与lnJ有相同极点。暂设x(j/k)已知，则令

可得误差协方差统计的MAP估值器为

(4.6)次优MAP估值器

在式(33)中，以滤波估值x(j/j)或预报估值x(j/j-1)近似代替计算复杂的平滑估值x(j/k)，可得次优MAP估值器。

(4.7)次优无偏MAP估值器

式中

所以其中，P_j(j/j-1)为X的预滤波估值协方差，P_j(j/j)为X的滤波估值协方差。

(4.8)引出递推无偏MAP估值器为

步骤五：分别计算k时刻匀速模型和“蛇形”机动模型的概率，并利用模糊逻辑算法进行模型概率更新。

(5.1)模糊概率更新模块的输入分别为步骤三输出的Λ₁和Λ₂，首先按照交互式多模型滤波算法计算模型概率的方式计算出这两个模型所对应的模型概率μ₁和μ₂，已知各模型前一时刻的概率为μ_j(k-1)，模型之间的转移矩阵为则

其中，

(5.2)设任一模型j，概率更新模块的输入变量为I₁和I₂，输出变量为u，令

(5.3)确定模糊输入、输出量。由于模型概率的取值范围为[0,1]，通过式(39)可以得出输入变量的论域范围分别为I₁:[0,1]、I₂:[-1,1]、u:[0,1]。设定了论域范围后，再在论域范围内划分输入输出变量的模糊子集，I₁的模糊子集为{小(S)，中(M)，大(B)}，I₂的模糊子集为{负(N)，零(Z)，正(P)}，u的模糊子集为{小(S)，中(M)，大(B)}。在确定了输入输出变量的模糊子集后，再确定每个变量的模糊子集中元素在对应变量论域范围内的隶属度。各个变量对应的隶属度函数如图2(a)-图2(c)所示，从左到右依次为I₁、I₂和u的隶属度函数。

(5.4)建立模糊规则库。根据输入、输出变量的含义以及经验知识可以得到如下结论：若模型概率变化为负，即I₂为N，则当前模型概率u应比前一时刻模型概率I₁小；若模型概率变化为零，即I₂为Z，则当前模型概率u应与前一时刻模型概率I₁相同；若模型概率变化为正，即I₂为P，则当前模型概率u应比前一时刻模型概率I₁大。则相应的模糊逻辑语句规则如下

Rule1：IF I₁ is S and I₂ is N，THEN u is S

Rule2：IF I₁ is S and I₂ is Z，THEN u is S

Rule3：IF I₁ is S and I₂ is P，THEN u is M

Rule4：IF I₁ is M and I₂ is N，THEN u is S

Rule5：IF I₁ is M and I₂ is Z，THEN u is M

Rule6：IF I₁ is M and I₂ is P，THEN u is B

Rule7：IF I₁ is B and I₂ is N，THEN u is M

Rule8：IF I₁ is B and I₂ is Z，THEN u is B

Rule9：IF I₁ is B and I₂ is P，THEN u is B

(5.5)计算模糊输出量u的模糊子集为{小(S)，中(M)，大(B)}，并将输出量反馈至滤波器。在对模糊逻辑系统的输出进行解模糊化时，采用中位数法进行解模糊化，得到模型的实际概率。

步骤六：当前时刻状态估计融合，利用获得的当前目标状态估计和模型概率计算总体状态输出。

(6.1)当前时刻的总体状态估计：

(6.2)当前时刻的总体状态误差协方差阵：

其中，μ_j(k)分别由步骤三、四、五输出。

本发明的效果可通过以下仿真进一步说明：

实验平台：因特尔i7处理器、主频2.20GHz、64位Windows 7专业版下的MatlabR2009a仿真软件。

(1)仿真参数

跟踪场景参数设置：三维情况下，目标在0-15s做匀速直线运动，16s-60s做“蛇形”运动。目标的机动频率ω＝0.2π已知，探测器周期为0.1s，龙格库塔法步长为0.01s，目标的非线性观测方程描述如下：Z(t)＝h[X(t)]+V(t)，

算法参数设置：目标初始值为[90000 2000 50 -1450 0 0 0 0 0]，两种运动模型初始概率都为0.5，初始状态误差协方差P(0)＝diag{[100 100 100 25 25 25 1 1 1]}，观测噪声的标准差为1mrad，蒙特卡洛仿真次数为50次。

(2)仿真内容

根据跟踪场景参数设置仿真三维动力学运动模型如图3所示。

图4(a)-图4(c)至图6(a)-图6(c)为分别采用本文改进FLIMM-AUKF算法、IMM-UKF算法和自适应IMM-UKF算法(石勇的算法)对目标进行跟踪滤波的位置、速度和加速度估计误差对比图。可以看出自适应算法和本文改进算法的误差明显小于IMM-UKF算法的估计误差。改进的FLIMM-AUKF算法效果比噪声自适应IMM-UKF效果好，这是因为改进的误差协方差算法不仅对误差协方差进行了自适应，同时也对噪声进行了自适应。并且本文改进算法在z方向上收敛速度明显快于自适应算法，误差收敛到±1，证明本文算法是可行的。

综上，本实施例提出了一种新的基于模糊控制多模型算法的目标跟踪方法，改进的跟踪算法能够快速的调整模型概率，使得其估计误差的收敛速度比交互式多模型算法更快，鲁棒性更强，并且能够有效降低纯方位目标跟踪的误差，实现对机动目标末端蛇形运动模型的稳定跟踪，能够满足当前的应用需求。

本领域技术人员可以理解，在本申请具体实施方式的上述方法中，各步骤的序号大小并不意味着执行顺序的先后，各步骤的执行顺序应以其功能和内在逻辑确定，而不应对本申请具体实施方式的实施过程构成任何限定。

最后应说明的是，以上实施例仅用以描述本发明的技术方案而不是对本技术方法进行限制，本发明在应用上可以延伸为其他的修改、变化、应用和实施例，并且因此认为所有这样的修改、变化、应用、实施例都在本发明的精神和教导范围内。

Claims (3)

1.一种基于模糊控制多模型算法的目标跟踪方法，其特征是：

步骤一：引入机动目标的动力学建模方法，对匀速模型和蛇形机动模型进行建模；

步骤二：对匀速模型和蛇形机动模型条件重新初始化，计算滤波器的混合输入；

步骤三：利用滤波器的混合输入，对匀速模型和蛇形机动模型采用无迹卡尔曼滤波算法进行并行滤波计算目标状态的初始值，并进行目标状态估计；

步骤四：采用改进的误差协方差统计估值器，递推估计系统误差协方差统计特性；

步骤五：分别计算k时刻匀速模型和蛇形机动模型的概率，并利用模糊逻辑算法进行模型概率更新；

步骤六：当前时刻状态估计融合，利用获得的当前目标状态估计和模型概率计算总体状态输出。

2.根据权利要求1所述的基于模糊控制多模型算法的目标跟踪方法，其特征是所述的引入机动目标的动力学建模方法，具体包括：

(1)对目标做蛇形机动进行动力学建模推导，机动加速度按正弦规律变化，目标坐标系中三个方向上的重力加速度和目标过载为：

$g_{x_2}=-gsin\mathrm{\θ}$

$g_{y_2}=-g\cos \mathrm{\θ}$

$g_{z_2}=0$

$a_{x_2}=0$

$a_{y_2}=-g_{y_2}$

$a_{z_2}=a_{0}cos\left(\mathrm{\ω}t\right)$

其中：和为重力加速度在目标坐标系中三个坐标轴方向的分量；和为目标过载在目标坐标系中三个坐标轴方向的分量；g为重力加速度；θ为目标的运动倾角；a₀为机动加速度幅度；ω为机动频率；

(2)采用龙格库塔法计算目标在惯性坐标系中的位置分量x、y和z，再经过坐标转换，将惯性坐标系转换到目标坐标系，得质心运动方程表示为：

$\stackrel{\·}{x}=v{\mathrm{cos\θcos\ψ}}_v$

$\stackrel{\·}{y}=vsin\mathrm{\θ}$

$\stackrel{\·}{z}=-v{\mathrm{cos\θsin\ψ}}_v$

$\stackrel{\·\·}{x}=\stackrel{\·}{v}=g_{x_2}+a_{x_2}$

$\stackrel{\·\·}{y}=v\stackrel{\·}{\mathrm{\θ}}=g_{y_2}+a_{y_2}$

$\stackrel{\·\·}{z}=-vcos\mathrm{\θ}{\stackrel{\·}{\mathrm{\ψ}}}_v=g_{z_2}+a_{z_2}$

其中：和为目标在惯性坐标系中的速度分量；和为目标在惯性坐标系中的加速度分量；v为目标的运动速率；为目标的运动速率的导数；ψ_v为目标的运动偏角；为目标的运动偏角的导数，为目标的运动倾角的导数。

3.根据权利要求1或2所述的基于模糊控制多模型算法的目标跟踪方法，其特征是所述采用改进的误差协方差统计估值器，递推估计系统误差协方差统计特性，具体包括：

(1)定义非线性离散时间系统：

$\left\{\begin{array}{c}X\left(k\right)=f\[X(k-1),k-1\]+G(k-1)W(k-1)\\ Z\left(k\right)=h\[X\left(k\right),k\]+V\left(k\right)\end{array}\right.$

其中：X(k)为k时刻n维被估计状态向量；Z(k)为k时刻m维观测向量；f[·]为n维可微向量函数；G(k-1)为k-1时刻n×r维过程噪声转移矩阵；W(k-1)为k-1时刻r维过程噪声；h[·]为m维可微向量函数；V(k)为k时刻m维观测噪声；

(2)计算误差协方差：

$P_x=E\{\[X\left(k\right)-\hat{X}\left(k\right)\]{\[X\left(k\right)-\hat{X}\left(k\right)\]}^T\}$

设误差协方差是未知的定常向量或矩阵，自适应滤波问题就是基于观测求误差协方差和状态X(k)；

(3)当误差协方差P_x未知时，连同状态X(0),…,X(k)的极大后验即MAP估计k时刻平滑估值x(j/k)用极大化条件概率密度求得：

J^*＝p[X(k),P_x|Z(k)]

式中，X(k)＝{x(0),x(1),…,x(k)}；Z(k)＝{z(1),z(2),…,z(k)}；p[·]表示变量概率；

由Bayes公式

$J^{*}=\frac{p\[X\left(k\right),P_x|Z\left(k\right)\]}{p\[Z\left(k\right)\]};$

(4)p[Z(k)]与最优化无关，转化为求如下无条件概率密度的极大值：

J＝p[X(k),P_x,Z(k)]＝p[Z(k)|X(k),P_x]·p[X(k)|P_x]·p[P_x]；

(5)设P_x服从均匀分布，由正态假设易知

$\begin{array}{c}p\[X\left(k\right)|P_x\]=p\[x\left(0\right)\]\underset{j=1}{\overset{k}{\mathrm{\Π}}}p\[x\left(j\right)|x(j-1),P_x\]\\ =d_1\left|P_0{|}^{-1/2}\right|P_x{|}^{-k/2}\exp \{-\frac{1}{2}||x\left(0\right)-x_0|{|}_{P_0^{-1}}^2-\frac{1}{2}\underset{j=1}{\overset{k}{\mathrm{\Σ}}}\left|\right|x\left(j\right)-f\[x(j-1),j-1\]\left|{|}_{P_x^{-1}}^2\right\}\end{array}$

有

$\begin{array}{c}p\[Z\left(k\right)\left|X\left(k\right)\right|P_x\]=\underset{j=1}{\overset{k}{\mathrm{\Π}}}p\[z\left(j\right)|x\left(j\right),R\]\\ =d_2\left|P_x{|}^{-k/2}\exp \right\{-\frac{1}{2}\underset{j=1}{\overset{k}{\mathrm{\Σ}}}\left|\right|z\left(j\right)-h\[x\left(j\right),j\]\left|{|}_{R^{-1}}^2\right\}\end{array}$

$J=c\left|P_x{|}^{-k}\exp \right\{-\frac{1}{2}\left|\right|x\left(0\right)-x_0|{|}_{P_0^{-1}}^2-\frac{1}{2}\underset{j=1}{\overset{k}{\mathrm{\Σ}}}||x\left(j\right)-f\[x(j-1),j-1\]|{|}_{P_x^{-1}}^2-\frac{1}{2}\underset{j=1}{\overset{k}{\mathrm{\Σ}}}\left|\right|z\left(j\right)-h\[x\left(j\right),j\]\left|{|}_{R^{-1}}^2\right\}$

$\ln J=-k\ln \left|P_x\right|-\frac{1}{2}\underset{j=1}{\overset{k}{\mathrm{\Σ}}}\left|\right|x\left(j\right)-f\[x(j-1),j-1\]|{|}_{P_x^{-1}}^2-\frac{1}{2}\underset{j=1}{\overset{k}{\mathrm{\Σ}}}||z\left(j\right)-h\[x\left(j\right),j\]|{|}_{R^{-1}}^2+const$

其中，x(j)为平滑估值；x(j-1)为预平滑估值；x(0)为初始状态；P₀为初始误差协方差；c,d₁,d₂,const均为常数；

J与lnJ有相同极点，设x(j/k)已知，则令

$\frac{\∂\ln J}{\∂P_x}=0$

得误差协方差统计的MAP估值器为

${\hat{P}}_x\left(k\right|k)=\frac{1}{k}\underset{j=1}{\overset{k}{\Σ}}\{x(j/k)-f\[x(j-1),k\]\}\·{\{x(j/k)-f\[x(j-1),k\]\}}^T$

(6)次优MAP估值器

以滤波估值x(j/j)或预报估值x(j/j-1)近似代替计算复杂的平滑估值x(j/k)，可得次优MAP估值器。

${\hat{P}}_x\left(k\right|k)=\frac{1}{k}\underset{j=1}{\overset{k}{\Σ}}\{x(j/j)-f\[x(j-1),j-1\]\}\·{\{x(j/j)-f\[x(j-1),j-1\]\}}^T$

(7)次优无偏极大后验估值器

$\mathrm{\ϵ}\left(k\right)=x\left(k\right)-f\[\hat{X}(k/k-1),k-1\]$

式中

$\begin{array}{c}E\[{\hat{P}}_x\left(k\right)\]=\frac{1}{k}\underset{j=1}{\overset{k}{\Σ}}K\left(j\right)E\[\mathrm{\ϵ}\left(j\right){\mathrm{\ϵ}}^T\left(j\right)\]K^T\left(j\right)\\ =\frac{1}{k}\underset{j=1}{\overset{k}{\Σ}}\[P_x(j/j-1)H^T\left(j\right)K^T\left(j\right)\]\\ =\frac{1}{k}\underset{j=1}{\overset{k}{\Σ}}\[P_x(j/j-1)-P_x(j/j)\]\end{array}$

所以其中，P_x(j/j-1)为X的预滤波估值协方差，P_x(j/j)为X的滤波估值协方差；

(8)引出递推无偏MAP估值器为

${\hat{P}}_x\left(k\right|k)=\frac{1}{k+1}\[k{\hat{P}}_x(k-1)-K\left(k\right)\mathrm{\ϵ}\left(k\right){\mathrm{\ϵ}}^T\left(k\right)K^T\left(k\right)\].$