一种应用于非线性高斯模型的平滑约束扩展卡尔曼滤波方法与流程

本发明涉及非线性滤波技术领域，具体是一种应用于非线性高斯模型的平滑约束扩展卡尔曼滤波方法。

背景技术：

在实际工程应用中，对于非线性随机动态系统状态估计，由于观测系统内在的非线性及外在测量环境的不确定性，最优卡尔曼滤波算法(kalmanfilter,kf)得不到解析解。扩展卡尔曼滤波(extendedkalmanfilter,ekf)算法的基本思想是在卡尔曼滤波估计点对非线性系统函数进行线性化处理。该算法由于实现简单，计算效率高，是目前在实际工程中是应用最广泛的非线性滤波算法。然而，由于线性化误差的存在，在动态系统非线性较强或测量噪声较大时，扩展卡尔曼滤波算法难以获得理论上的稳态解，滤波性能下降甚至发散。针对该问题，正交卡尔曼滤波(quadraturekalmanfiltering,qkf)算法避免点估计近似，相应地计算量增大；另外还有文献提出了多模态正交卡尔曼滤波(multiplequadraturekalmanfiltering,mqkf)，将原始空间划分为若干子空间，采用gauss-hermite正交规则传递有效子空间的点集，利用正交卡尔曼滤波提供的估计不确定性来改善滤波器之间的相互作用，同样存在运算量大的问题。

例如：

专利号cn103312297a公开了一种迭代扩展增量卡尔曼滤波方法，通过利用量测方程的增量形式消除了量测方程中未知的系统误差，从而建立了精确的增量量测方程；在此基础之上，利用迭代方法来优化对非线性量测方程进行线性化产生的误差。上述技术方案虽然在一定程度上减少了发散现象，但是同样难以获得理论上的稳态解；

专利号cn102788976a公开了一种高量级扩展卡尔曼滤波方法，将状态的初始滤波值中的元素和距离的量测值均用高量级表示，因为使用了更高的量级，滤波值和距离量测值在数值上减小了，也就是将实际中目标的远距离转换成数据处理中的“近距离”。距离在数值上的减小降低了滤波中坐标转换的非线性，从而在使用扩展卡尔曼滤波时线性化误差减小，滤波精度虽然比没有使用量级转换的经典扩展卡尔曼滤波要高。但是上述方案无法做到控制卡尔曼滤波增益为有界，避免在状态更新过程中计算新息协方差可能出现病态矩阵。

技术实现要素：

针对上述现有技术中的不足，本发明提供一种应用于非线性高斯模型的平滑约束扩展卡尔曼滤波方法。

为实现上述目的，本发明提供一种应用于非线性高斯模型的平滑约束扩展卡尔曼滤波方法，包括如下步骤：

步骤一：建立约束离散非线性随机系统

对于非线性随机系统的状态估计问题，离散时间状态模型包括系统状态过程和系统测量模型，其中，系统状态过程模型将当前时刻状态向量和前向一时刻状态向量相关联，系统测量模型将状态向量和测量向量相关联，具体为：

xk+1＝fk(xk)+vk(1)

zk＝hk(xk)+ek(2)

式(1)-(2)中，k是跟踪时刻；表示k时刻nx维的系统状态向量；是k时刻nz维的量测向量；fk与hk分别表示已知的状态转移矩阵和测量矩阵；vk和ek分别是过程噪声和测量噪声；

对于物理意义上系统统计噪声，数学上，根据最小二乘方法优化状态估计，即：

式(3)中，k表示系统观测总时长，λ表示衡量过程噪声和量测噪声的动态因子；

步骤二：约束正则化非线性随机系统可行域

给定测量序列zk，通过递推估计得到目标状态的最大后验分布p(xk|z1:k)，进而得到状态向量满足约束条件的最大后验概率估计，为：

式(4)中，上标c表示约束，ln表示取自然对数，为第k时刻目标状态后验概率密度函数准确的有效区域；

根据对偶原理，最大化等价于最小化因此得到状态向量的可行域中心点的目标函数为：

式(5)中，i表示第i个估计，分别是-logp(xk|z1:k)在点处的雅可比和海森矩阵；

将状态变量倒数的自然对数为障碍函数，对状态向量的可行域中心点的目标函数进行正则化，将状态向量的可行域中心点的目标函数转化为序列无约束优化增广目标函数，即：

式(6)中，xk为状态变量，ρ∈(0,1)是罚函数因子；

基于序列无约束优化增广目标函数得到状态向量的可行集：

式(7)中，为可行域的均值，pxx为可行域的协方差；

步骤三，约束非线性系统更新

联立式子(1)和(7)，传递状态可行集

式(8)中，表示约束条件下，可行状态集的传递；

在可行域内，状态传递过程的雅可比为：

式(9)-(10)中，表示约束条件下的状态转移矩阵，gk表示系统过程噪声的雅可比，fk表示系统状态传递函数；

由下式计算约束条件下新息协方差为：

式(11)中，qk表示第k时刻的过程噪声的标准方差；

在可行域内，测量模型的雅可比为：

式(12)-(13)中，表示约束条件下的测量矩阵，uk表示系统观测噪声的雅可比，hk表示系统观测方程，表示系统观测噪声均值；

得到约束条件下卡尔曼滤波增益为：

式(14)中，rk表示第k时刻的量测噪声的标准方差；

得到状态更新的均值和协方差，为：

作为上述技术方案的进一步改进，步骤一中，过程噪声和测量噪声为互不相关的高斯噪声，均值为零。

作为上述技术方案的进一步改进，步骤二中，第k时刻目标状态后验概率密度函数准确的有效区域定义为：

式中，是dz维连通的量测区域。

作为上述技术方案的进一步改进，步骤二中，得到状态向量满足约束条件的最大后验概率估计的具体过程为：

给定测量序列zk，通过递推估计得到目标状态的最大后验分布p(xk|z1:k)，得到状态向量的最大后验估计：

基于对数运算的结构不变性，采用状态向量的最大后验估计的对数形式作为状态向量满足约束条件的最大后验概率估计：

作为上述技术方案的进一步改进，步骤二中，基于序列无约束优化增广目标函数得到状态向量的可行集的具体过程为：

将状态向量的初始值记为xk，采用拟牛顿迭代得到最优解，即：

式中，α^*是步长，dk是搜索方向，fk,o是第k时刻的序列无约束优化增广目标函数；

综上，状态向量的可行域近似为均值为协方差为pxx的高斯分布，即：

作为上述技术方案的进一步改进，其特征在于，步长α^*＝1。

为实现上述目的，本发明还提供一种应用于非线性高斯模型的平滑约束扩展卡尔曼滤波系统，包括存储器和处理器，所述存储器存储有计算机程序，所述处理器执行所述计算机程序时实现上述方法的步骤。

为实现上述目的，本发明还提供一种计算机可读存储介质，其上存储有计算机程序，所述计算机程序被处理器执行时实现上述的方法的步骤。

本发明提供的一种应用于非线性高斯模型的平滑约束扩展卡尔曼滤波方法，对非线性随机系统的统计测量噪声建立双边限幅的约束模型，根据最大后验估计思想构建目标函数，通过非线性最小二乘牛顿迭代方法近似求解可行集，选择可行集并进行传递和更新，从而避免了非线性系统高阶海森矩阵的计算及状态更新过程中计算新息协方差可能出现的不可逆病态矩阵，保证系统的卡尔曼滤波增益为有界、统计误差收敛。

附图说明

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

图1为本发明实施例中应用于非线性高斯模型的平滑约束扩展卡尔曼滤波方法的流程示意图；

图2为本发明实施例中应用于非线性高斯模型的平滑约束扩展卡尔曼滤波方法的框架简图。

图3为本发明实施例中基点误差的示意图；

图4为本发明实施例仿真实验一中不同滤波方法的系统状态估计与真实状态的对比曲线。

图5为本发明实施例仿真实验一中不同滤波方法的均方根估计误差曲线；

图6为本发明实施例仿真实验二中σv＝0.005kms^-2时不同滤波方法位置均方根误差随时间增长的变化趋势曲线；

图7为本发明实施例仿真实验二中σv＝0.01kms^-2时不同滤波方法位置均方根误差随时间增长的变化趋势曲线；

图8为本发明实施例仿真实验二中σv＝0.04kms^-2时不同滤波方法位置均方根误差随时间增长的变化趋势曲线；

图9为本发明实施例仿真实验二中σe＝3mrad时不同滤波方法跟踪结果的位置均方根误差曲线；

图10为本发明实施例仿真实验二中σe＝5mrad时不同滤波方法跟踪结果的位置均方根误差曲线；

图11为本发明实施例仿真实验二中过程噪声对方法滤波性能的影响中第二组参数的状态估计在3维空间中x方向上的位置均方根误差曲线；

图12为本发明实施例仿真实验二中过程噪声对方法滤波性能的影响中第二组参数的状态估计在3维空间中y方向上的位置均方根误差曲线；

图13为本发明实施例仿真实验二中过程噪声对方法滤波性能的影响中第二组参数的状态估计在3维空间中z方向上的位置均方根误差曲线。

本发明目的的实现、功能特点及优点将结合实施例，参照附图做进一步说明。

具体实施方式

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

需要说明，本发明实施例中所有方向性指示(诸如上、下、左、右、前、后……)仅用于解释在某一特定姿态(如附图所示)下各部件之间的相对位置关系、运动情况等，如果该特定姿态发生改变时，则该方向性指示也相应地随之改变。

另外，在本发明中如涉及“第一”、“第二”等的描述仅用于描述目的，而不能理解为指示或暗示其相对重要性或者隐含指明所指示的技术特征的数量。由此，限定有“第一”、“第二”的特征可以明示或者隐含地包括至少一个该特征。在本发明的描述中，“多个”的含义是至少两个，例如两个，三个等，除非另有明确具体的限定。

在本发明中，除非另有明确的规定和限定，术语“连接”、“固定”等应做广义理解，例如，“固定”可以是固定连接，也可以是可拆卸连接，或成一体；可以是机械连接，也可以是电连接，还可以是物理连接或无线通信连接；可以是直接相连，也可以通过中间媒介间接相连，可以是两个元件内部的连通或两个元件的相互作用关系，除非另有明确的限定。对于本领域的普通技术人员而言，可以根据具体情况理解上述术语在本发明中的具体含义。

另外，本发明各个实施例之间的技术方案可以相互结合，但是必须是以本领域普通技术人员能够实现为基础，当技术方案的结合出现相互矛盾或无法实现时应当认为这种技术方案的结合不存在，也不在本发明要求的保护范围之内。

如图1-2所示的一种应用于非线性高斯模型的平滑约束扩展卡尔曼滤波方法，通过对非线性动态系统的状态预测更新进行预处理，控制卡尔曼滤波增益为有界，避免在状态更新过程中计算新息协方差可能出现病态矩阵，具体包括如下步骤：

步骤一：建立约束离散非线性随机系统

对于非线性随机系统的状态估计问题，离散时间状态模型包括系统状态过程和系统测量模型，其中，系统状态过程模型将当前时刻状态向量和前向一时刻状态向量相关联，由一阶差分方程描述，测量模型将状态向量和测量向量相关联，具体为：

xk+1＝fk(xk)+vk(1)

zk＝hk(xk)+ek(2)

式(1)-(2)中，k是跟踪时刻；表示k时刻nx维的系统状态向量；是k时刻nz维的量测向量；fk与hk分别表示已知的状态转移矩阵和测量矩阵；vk和ek分别是过程噪声和测量噪声，其中，过程噪声和测量噪声为互不相关的高斯噪声，均值为零，差分别为σk和rk；

实际上，对于非线性随机系统状态估计，在物理测量过程中，数据传输与意图信息存在着不完整性及不确定性，数学模型上可通过统计测量噪声和过程噪声表示。根据高斯-马尔可夫定理，最小二乘估计是最有效的可能估计，因此对于物理意义上系统统计噪声，数学上，根据最小二乘方法优化状态估计，即：

式(3)中，k表示系统观测总时长，λ表示衡量过程噪声和量测噪声的动态因子；

鉴于噪声通常是能量有限的，第k时刻目标状态后验概率密度函数准确的有效区域可以定义为：

式中，是dz维连通的量测区域，其中，p(vk)表示系统过程噪声概率密度函数，p(ek)表示系统观测噪声概率密度函数。

步骤二：约束正则化非线性随机系统可行域

基于线性化的非线性滤波方法是对状态最新估计值进行泰勒级数展开，这引入了两种类型的误差，即，由于忽略高阶项而引起的截断误差和线性化估计值偏离状态真值的基点误差。由于协方差矩阵描述了状态空间中估计的变化方向，如果状态估计值偏离真值所在的超平面，将会导致系统滤波计算发散，即如图3所示。

因此在实际的物理系统中，由于动态系统状态信息之间存在约束，滤波器需要考虑：

(1)能够将系统模型与约束条件相结合；

(2)递归以提供最新状态估计。

对于第一个要求，卡尔曼滤波方法可以将线性约束视为“准确”(perfect)测量。然而，这可能会产生奇异的状态协方差矩阵，从而导致系统计算发散。为此，本实施例提出平滑约束扩展卡尔曼滤波方法，基于最小二乘准则对系统统计噪声建立约束模型，见式(3)。在贝叶斯框架下，根据最小化估计误差构建目标代价函数。

给定测量序列zk，通过递推估计得到目标状态的最大后验分布p(xk|z1:k)，得到状态向量的最大后验估计：

最大后验概率估计器利用到了关于状态向量的所有时刻的测量信息，从而加强了数值优化求解的唯一性和稳定性。数学上，由于对数运算具有结构不变性，因此采用系统状态后验概率密度函数的对数形式比直接采用后验概率密度函数要方便。相应地，采用状态向量的最大后验估计的对数形式作为状态向量满足约束条件的最大后验概率估计：

式(4)中，上标c表示约束，ln表示取自然对数，z1:k是所有时刻(包括历史时刻)的观测，为第k时刻目标状态后验概率密度函数准确的有效区域。

在贝叶斯滤波框架下，平滑约束扩展卡尔曼滤波方法的目标是由观测序列矢量逼近状态最大后验分布。该方法的关键步骤是确定可行域中心，通过遍历搜索可行域逼近全局最优解。根据对偶原理，式(4)最大化等价于最小化其中，logp同式(4)中的lnp，因此得到状态向量的可行域中心点的目标函数为：

式(5)中，i表示第i个估计，分别是-logp(xk|z1:k)在点处的雅可比和海森矩阵；

为了近似计算满足非线性状态约束的状态预测，通过正则化目标函数进行数值求解，即将状态变量倒数的自然对数为障碍函数，对状态向量的可行域中心点的目标函数进行正则化，将状态向量的可行域中心点的目标函数转化为序列无约束优化增广目标函数，即：

式(6)中，xk为状态变量，ρ∈(0,1)是罚函数因子；通过式(6)保持每一个迭代点xk是可行域的内点，对于不满足约束区域的点，当迭代点靠近边界时，增广目标函数值骤然增大以示“惩罚”，从而阻止其穿越边界。

基于序列无约束优化增广目标函数得到状态向量的可行域：

首先将状态向量的初始值记为x0，采用拟牛顿迭代得到最优解，即：

式中，α^*是步长，dk是搜索方向，fk,o是第k时刻的序列无约束优化增广目标函数，本实施例中步长α^*＝1；

综上，状态向量的可行域近似为均值为协方差为pxx的高斯分布，即：

式(7)中，为可行域的均值，pxx为可行域的协方差。

步骤三，约束非线性系统更新

联立式子(1)和(7)，传递状态可行域

式(8)中，表示约束条件下，可行状态集的传递；

在可行域内，状态传递过程的雅可比为：

式(9)-(10)中，表示约束条件下的状态转移矩阵，gk表示系统过程噪声的雅可比，fk表示系统状态传递函数；

由下式计算约束条件下新息协方差为：

式(11)中，qk表示第k时刻的过程噪声的标准方差；

在可行域内，测量模型的雅可比为：

式(12)-(13)中，表示约束条件下的测量矩阵，uk表示系统观测噪声的雅可比，hk表示系统观测方程，表示系统观测噪声均值；

得到约束条件下卡尔曼滤波增益为：

式(14)中，rk表示第k时刻的量测噪声的标准方差；

得到状态更新的均值和协方差，为：

其中，协方差矩阵能够表征状态估计是否准确。在上述状态更新的过程中，平滑约束扩展卡尔曼滤波方法通过约束正则对输入控制进行预处理，并将先验约束信息和最新量测引入到状态预测和更新过程，将新息协方差和滤波增益约束为有上确界，使得该方法在理论上满足计算收敛，能够得到状态的近似稳态解。

为了验证平滑约束扩展卡尔曼滤波方法的性能，将其与其它传统的非线性滤波方法进行比较，在单变量非平稳增长模型和被动机动目标跟踪两种仿真场景中进行蒙特卡洛仿真实验，每次随机起始并运行100次。

仿真实验一选为单变量非平稳增长模型(univariatenonstationarygrowthmodel,ungm)，系统方程为

式中，过程噪声服从gamma分布，即vk～gamma(3，2)；量测噪声是均值为0的高斯噪声，即nk～n(0，0.1)。

比较的方法包括扩展卡尔曼(extendedkalmanfilter,ekf)滤波、无迹卡尔曼滤波(unscentedkalmanfilter，ukf)、标准粒子滤波(genericalparticlefilter,gpf)，以及扩展卡尔曼粒子滤波(particlefilter-extendedkalmanfilter，pf-ekf)与无迹卡尔曼粒子滤波(particlefilter-unscentedkalmanfilter，pf-ukf)，分别采用ekf和ukf产生重要性密度函数。对于基于粒子滤波，粒子数目均设置为n＝200，采用残差重采样法。滤波性能比较参数选为均方根误差(rootmeansquareerror,rmse)。令和分别表示在第k时刻第i次蒙特卡洛实验中目标的真实状态与估计状态，假设总共运行m次蒙特卡洛仿真实验，在k时刻，状态估计的均方根误差的定义为：

图4给出了不同滤波方法的系统状态估计与真实状态的对比曲线，图5是不同滤波方法的均方根估计误差曲线。表1统计了不同滤波方法的均方根误差统计量，及独立运行100次蒙特卡洛仿真实验所需时间。

表1rmse及运行时间比较

定性比较和定量统计结果均显示，提出的平滑约束扩展卡尔曼滤波方法的滤波性能明显优于对比的其它次优滤波方法。扩展卡尔曼滤波方法基于泰勒级数展开，由于在预测协方差方程时忽略高阶项误差，因此，在系统非线性较强的情况下，估计性能下降明显。分析仿真实验结果的统计数据可知，平滑约束扩展卡尔曼滤波方法的滤波精度高于无迹卡尔曼滤波方法，接近于无迹卡尔曼粒子滤波方法，这是由于平滑约束扩展卡尔曼滤波方法利用约束优化，遍历可行域近似求解全局最优解，选择可行域进行状态传递和滤波更新，从而提高了系统状态估计的准确性。在计算速度方面，平滑约束扩展卡尔曼明显优于粒子滤波方法，这是由于在迭代求解过程中通过最速下降梯度方向搜索，计算收敛速度较快。

仿真实验二选为被动机动目标跟踪，仿真场景设计及参数设置同文献“hongweiz,weixinxie.constrainedauxiliaryparticlefilteringforbearings-onlymaneuveringtargettracking[j].journalofsystemsengineeringandelectronics,2019,30(4):684-695.”的仿真实验一。比较方法包括扩展卡尔曼滤波(ekf)、正交卡尔曼滤波(qkf)和混合正交卡尔曼滤波(mqkf)，采用控制变量法比较过程噪声、测量噪声等内在和外在因素对方法滤波性能的影响。目标状态方程选为

式中,t是采样间隔，vk是3×1独立同分布的过程噪声，vk～n(0,q)，均值为0，方差为q＝σ²i，i是3×3单位矩阵，σ是vk的标准方差。

选用两个固定平台上的静态无源传感器提供角度量测，假定同步发生并且无传输延迟，各自的量测信息通过数据链路进行传输连接。假设目标检测概率是统一的且没有错误警报(因此忽略数据关联问题)。θj,k和βj,k分别表示第k时刻由第j个传感器提供的方位和俯仰角。在三维(3d)状态空间中，第k时刻由第j个传感器测得的目标状态表示为其中，(xj,k,yj,k,zj,k)和分别表示目标在(x,y,z)轴上的位置，是第j个传感器位置。系统观测模型可重新写为如下形式

式中，传感器的量测噪声假设为0均值的高斯噪声，即各自相互独立，且与过程噪声σv及模型状态无关。两个被动传感器分别位于坐标系的y坐标处为(0,5km,0)和(0,-5km,0)。

滤波性能比较参数选为均方根误差(rootmeansquareerror,rmse)。令和分别表示第k时刻第i次蒙特卡洛实验中目标的实际位置与估计位置。假设运行m次蒙特卡洛仿真实验。在第k时刻，状态估计的位置均方根误差的定义为：

过程噪声对方法滤波性能的影响：固定测量噪声标准差为1.5mrad，采样时间间隔t＝1s。在一定范围内增大过程噪声标准差，选为0.005km.s-²、0.01km.s-²及0.04km.s-²三组。图6为σv＝0.005kms^-2时不同滤波方法位置均方根误差随时间增长的变化趋势(rmse)曲线，图7为σv＝0.01kms^-2时不同滤波方法位置均方根误差随时间增长的变化趋势(rmse)曲线，图8为σv＝0.04kms^-2时不同滤波方法位置均方根误差随时间增长的变化趋势(rmse)曲线。仿真结果的定性比较结果显示，随着过程噪声的增加，在目标作匀速运动阶段，所有滤波器的跟踪性能变差。而在目标作匀转弯飞行的机动运动阶段，所有滤波方法的跟踪性能变得更好。这是因为，过程噪声驱动状态传递，一定范围内增大过程噪声，对机动运动建模更符合物理情况。由定量统计均值比较可知，位置均方根误差从大到小依次为扩展卡尔曼滤波、正交卡尔曼滤波、混合正交卡尔曼滤波和平滑约束扩展卡尔曼滤波。

测量噪声的影响对方法滤波性能的影响：固定过程噪声标准差为σv＝0.01km.s^-2，观测时间间隔t＝1s。在一定范围内增大测量噪声标准差，选为σe＝1.5mrad、σe＝3mrad及σe＝5mrad三组。其中，量测噪声标准差为σe＝1.5mrad的仿真结果与图7情况相同。图9为σe＝3mrad时不同滤波方法跟踪结果的位置均方根误差曲线、图10为σe＝5mrad时不同滤波方法跟踪结果的位置均方根误差曲线。仿真实验结果表明，随着量测噪声的增加，比较的所有滤波方法的估计偏差变大，而cekf滤波方法rmse最小，且变化平稳。

为了比较方法的整体滤波性能，图11-13分别给出了过程噪声对方法滤波性能的影响中第二组参数的状态估计在3维空间中x、y和z方向上的位置均方根误差曲线。

从图11-13可知，图11、图12的位置均方根误差对比差别比较明显，图13差距较小。这主要是因为仿真场景设置是目标在一定高度的二维平面上航行，所以机动运动主要在x、y方向发生。该仿真实验结果进一步证明了平滑约束扩展卡尔曼滤波方法有效提高了非线性动态系统的滤波准确性，显著增强了计算稳定性。

表2.100次蒙特卡洛仿真实验的平均时间

表2给出了过程噪声对方法滤波性能的影响的第二组参数情形下，运行100次蒙特卡洛实验所需的平均时间。相比于扩展卡尔曼滤波方法，本实施例所提出的平滑约束扩展卡尔曼滤波方法在提高滤波精度和稳定性的前提下，增加了更多的额外计算量。而相比于正交扩展卡尔曼及混合正交扩展卡尔曼滤波方法，该滤波方法提供了在线实时跟踪机动目标的能力。

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