一种非线性目标跟踪系统中平方根容积卡尔曼滤波方法与流程

本发明属于数据融合、滤波算法等技术领域，涉及一种平方根容积卡尔曼滤波方法，特别涉及一种非线性目标跟踪系统中平方根容积卡尔曼滤波方法。

背景技术：

传感器在对目标跟踪系统进行监测时会受到随机噪声干扰，卡尔曼滤波算法可以利用系统离散系统的状态方程与量测方程对系统进行最优估计，减少随机噪声干扰造成的影响(文献1)。常规的卡尔曼滤波算法只适用于线性离散系统，arasaratnam和haykin两位学者提出了适用于非线性离散系统的平方根容积卡尔曼滤波(square-rootcubaturekalmanfilter，sckf)算法，提高了滤波稳定性与精确性(文献2)，但在噪声统计特性未知、系统动力学特性的数学模型不准确的环境中时，此算法会因模型与获得的量测值不匹配而导致滤波发散。李宁等人提出了一种自适应平方根容积卡尔曼滤波算法(adaptivesquare-rootcubaturekalmanfilter，asckf)(文献3)，其将sage-husa时变噪声估计器运用到sckf算法中，可以在利用量测数据进行递推滤波时，通过时变噪声估计器，每次滤波时都同时估计和修正系统噪声协方差矩阵qk和量测噪声协方差矩阵rk的统计特性，在一定程度上克服sckf算法在噪声统计特性未知和不确定时滤波精度较低甚至发散的缺点(文献4)，但实际上，sage-husa时变噪声估计器每次滤波需要在已知qk的情况下对rk进行估计，而不能在qk与rk都未知时同时把它们估计出来(文献5)，故随着滤波次数的增加，asckf算法滤波精确性可能会降低且滤波的复杂度增加导致实时性降低。鲁平等人提出了一基于协方差匹配技术的滤波发散判据，其只在滤波异常时对rk进行估计，在保证精度与敛散性的前提下，计算的复杂度降低，能够提高系统的实时性(文献6)。但鲁平提出的滤波发散判据只适用于线性系统，并不能够直接与asckf相结合去解决非线性目标跟踪系统的滤波精确性降低和实时性降低的问题。[文献7]介绍的自适应强跟踪卡尔曼滤波算法中，提出利用雅克比矩阵将非线性系统近似等效于线性系统的思想，效果较好，但强跟踪算法计算复杂，实时性差。

在目标跟踪系统实际应用中，多为非线性系统，主要问题包含两方面：

(1)通常情况下，环境变化等诸多因素会使得系统难以得到精确的数学模型和噪声统计特性，例如，船舶动力定位系统在海面上运行时，风、浪等因素干扰都会使船体的运动状态不可能完全稳定，此时系统运动数学模型与噪声统计特性难以精确给定。

(2)实际应用时，目标跟踪系统需要在一个采样周期里完成传感器测参数、处理器滤波等操作，必须尽可能减少滤波计算时间，保证在一个采样周期里完成所有导航计算。因此改进的算法提高了系统的稳定性与实时性，对实际应用是十分有益的。

综上所述，需要一种能运用到非线性系统的具备良好性能的噪声估计器性能的滤波算法，在保证滤波精确的同时，还需要进一步简化算法，以提高算法实时性。

[文献1]彭丁聪.卡尔曼滤波的基本原理及应用[j].软件理论与方法,2009,(11):1-2.

[文献2]arasaratnami,haykins.cubaturekalmanfilters[j].ieeetrans.onautomaticcontrol,2009,54(6):1254-1269.

[文献3]李宁，祝瑞辉，张勇刚.基于sage_husa算法的自适应平方根ckf目标跟踪方法[j].系统工程与电子技术，2014，36(10).

[文献4]lichtfusee.non-gpsnavigationusingvision-aidingandactiveradiorangemeasurements[d].departmentoftheairforceairuniversity,airforceinstituteoftechnology,wright-pattersionairforcebase,ohio,usa,2011.

[文献5]张常云.自适应滤波方法研究[j].航空学报,1998,19(7):96-99.

[文献6]鲁平,赵龙,陈哲.改进的sage-husa自适应滤波及其应用[j].系统仿真学报,2007,19(15):3503-3505

[文献7]zhenjiepan,lianggao,sheshenggaoandbingbinggao.adaptivecubaturekalmanfilterforultra-tightlycoupledbds/insintegration[j].proceedingsof2016ieee6thinternationalconferenceonelectronicsinformationandemergencycommunication,2016,269-272.

技术实现要素：

为了解决上述技术问题，本发明提供了一种非线性系统自适应平方根容积卡尔曼滤波算法(adaptivesquare-rootcubaturekalmanfilterofnonlinearsystem，asckfns)，其利用雅克比矩阵将非线性系统近似转换成线性系统的方法改进滤波发散判据，使其能与asckf算法结合运用到非线性系统目标跟踪算法中去，并适当简化计算量，提高算法的精确性与实时性。

本发明所采用的技术方案是：一种非线性目标跟踪系统中平方根容积卡尔曼滤波方法，其特征在于：针对自适应平方根容积卡尔曼滤波算法asckf，改进时间更新环节与sage-husa噪声估计器环节，形成asckfns算法；

在时间更新环节，将非线性状态转移函数f根据求一阶偏导的原理求雅克比矩阵代入第k-1次采样时刻的状态向量后验估计值到雅克比矩阵中，得到第k次采样时刻将非线性系统近似线性化的等效状态矩阵fk，简化非线性系统的状态转移函数，从而简化时间更新环节，提高系统实时性；

在sage-husa噪声估计器环节，将非线性目标跟踪系统量测函数h根据求一阶偏导的原理求雅克比矩阵代入k时刻的先验状态向量预测值xk|k-1到雅克比矩阵中，得到第k次采样时刻将非线性系统近似线性化的等效量测矩阵hk，进而得到较为简单的噪声敛散判决，在进行第k次滤波时，让噪声敛散判决同时作用于系统噪声协方差矩阵qk与量测噪声协方差矩阵rk，检验是否需要修正qk与rk，这样不用每次滤波都根据系统噪声协方差矩阵qk对量测噪声协方差矩阵rk进行修正，防止长期同时估计qk与rk造成滤波异常。

本发明适用于非线性离散系统，在保证算法的稳定性的前提下，进一步提高了算法精确性，且大量减小了计算量，提高了实时性。

附图说明

图1为本发明实施例的流程图；

图2为本发明实施例的asckfns算法、asckf算法与真实运动轨迹对比图；

图3为本发明实施例的状态向量asckfns算法、asckf算法与真实值的对比图；

图4为本发明实施例的asckfns算法、asckf算法状态向量均方根误差对比图。

具体实施方式

为了便于本领域普通技术人员理解和实施本发明，下面结合附图及实施例对本发明作进一步的详细描述，应当理解，此处所描述的实施示例仅用于说明和解释本发明，并不用于限定本发明。

针对一般的非线性目标跟踪系统进行分析，其数学模型可描述为：

xk＝f(xk-1)+wk-1(1)；

zk＝h(xk)+vk(2)；

式(1)中，xk为第k次采样时刻系统状态向量，f(x)为系统转移函数，wk-1为第k-1次采样时刻系统噪声向量。

式(2)中，zk为第k次采样时刻系统量测向量，h(x)为系统量测函数，vk为第k次采样时刻量测噪声向量。

系统噪声向量与量测噪声向量之间的关系为：

式(3)中，表示系统噪声与量测噪声互不相关，两者期望值为0，系统噪声协方差矩阵为q，量测噪声协方差矩阵为r。

基于上述的非线性目标跟踪系统，请见图1，本发明提供的一种非线性目标跟踪系统中平方根容积卡尔曼滤波方法，针对自适应平方根容积卡尔曼滤波算法asckf，改进时间更新环节与sage-husa噪声估计器环节，形成asckfns算法；具体包括以下步骤：

(1)时间更新；

将非线性函数f()在点进行近似线性化：

计算状态先验预测值：

计算误差协方差矩阵先验预测值：

计算误差协方差矩阵先验预测值的平方根：

sk|k-1＝chol(pk|k-1)(7)

式(7)中，chol()为对矩阵进行因式分解。

(2)量测更新；

计算容积点权值：

式(8)中，m为状态向量的维数；i为单位矩阵；[i,-i]j表示取矩阵[i,-i]的第j列；ωj为容积点权值。

计算容积点：

xj,k|k＝sk|k-1ξj+xk|k-1(9)；

传播容积点：

zj,k|k-1＝h(xj,k|k)(10)；

计算量测先验预测值：

(3)敛散判决计算；

计算残差值：

vk＝zk-zk|k-1(12)；

将非线性函数h()在点xk|k-1处进行近似线性化：

进行量测噪声敛散判决：

(4)敛散判决对量测噪声进行估计；

若式(14)成立，则取值为：

计算遗忘因子参数值：

式中，b常取值0.95～0.995

若式(14)不成立，则分别取值为：

(5)量测更新；

计算新息协方差矩阵的平方根：

式(20)中，为量测加权中心矩阵；tria()为对矩阵进行qr变换。

计算滤波增益值：

式(23)中，χk|k-1为系统加权中心矩阵；pxz,k|k-1为互协方差矩阵。

计算状态后验估计值：

计算误差协方差矩阵后验预测值：

pk|k＝sk|ksk|k^t(26)；(6)敛散判决对系统噪声进行估计；

若式(14)成立，则取值为：

若式(14)不成立，则取值为：

(7)算法结束判决

k≤n(29)；

式中，n为设定的算法最大采样次数

若式(29)成立，则继续按步骤(1)～(7)顺序依次进行计算。

若式(29)不成立，则结束计算。

本发明在asckf算法基础上进行改进，针对非线性离散系统，主要在时间更新环节与sage-husa噪声估计器环节对其进行改进，形成asckfns算法，以提高系统精确性与实时性。asckfns算法包含三个环节：时间更新环节、量测更新环节、sage-husa噪声估计器环节，asckfns算法主要改进思想为：

(1)在时间更新环节，可以考虑将非线性状态转移函数f根据求一阶偏导的原理求雅克比矩阵代入第k-1次采样时刻的状态向量后验估计值到雅克比矩阵中，可得到第k次采样时刻将非线性系统近似线性化的等效状态矩阵fk，这样就可以将非线性系统的状态转移函数进行了简化，从而简化时间更新环节，提高系统实时性。

sk|k-1＝chol(pk|k-1)；

式中，为第k-1次采样时刻得到的滤波后系统噪声协方差矩阵qk-1的估计值；pk-1|k-1为第k-1次采样时刻得到的误差协方差矩阵后验预测值；pk|k-1为第k次采样时刻得到的误差协方差矩阵先验预测值；chol()表示对矩阵进行cholesky分解，把一个对称正定的矩阵表示成一个下三角矩阵和其转置的乘积，例如若chol(pk|k-1)＝sk|k-1，则满足sk|k-1sk|k-1^t＝pk|k-1；

asckfns算法的时间更新环节相比于asckf算法的时间更新环节，没有了容积点的求取，减少了计算量，两者具体的计算量对比在后文中进行具体介绍。

(2)在sage-husa噪声估计器环节，将非线性系统量测函数h根据求一阶偏导的原理求雅克比矩阵代入k时刻的先验状态向量预测值xk|k-1到雅克比矩阵中，可得到k时刻将非线性系统近似线性化的等效量测矩阵hk，进而得到较为简单的噪声敛散判决，在进行第k次滤波时，让噪声敛散判决同时作用于系统噪声协方差矩阵qk与量测噪声协方差矩阵rk，检验是否需要修正qk与rk，这样不用每次滤波都根据系统噪声协方差矩阵qk对量测噪声协方差矩阵rk进行修正，防止长期同时估计qk与rk造成滤波异常。敛散判决推导过程为：

vk≈zk-hkxk|k-1；

可得敛散判决为：

式中，vk为第k次采样时刻的新息，zk为第k次采样时刻系统量测向量；为第k-1次采样时刻得到的滤波后系统噪声qk-1的估计值；tr[]为对矩阵求迹。

由于系统噪声协方差矩阵qk随着滤波次数的增加会逐渐趋于稳定，本发明着重估计量测噪声协方差矩阵rk，让噪声敛散判决同时作用于qk与rk，可以简化计算量。并在sage-husa噪声估计器的递推公式中适当省去负项，这样可以进一步防止误差协方差矩阵非正定无法分解的情况，进一步减少计算量，提高算法实时性。

若上述判决成立，表明量测噪声引起的状态估计值的实际误差将超过理论预计值，需要对量测噪声协方差矩阵rk进行修正，则与取值分别为：

式中，为第k次采样时刻得到的滤波后量测噪声协方差矩阵rk的估计值；为第k次采样时刻得到的滤波后系统噪声协方差矩阵qk的估计值；dk-1为遗忘因子，加强新数据对参数估算的影响力；kk为第k次采样时刻的卡尔曼滤波增益。

若上述判决不成立，表明实际误差没超过理论预计值，不需要对量测噪声协方差矩阵rk进行修正，则与取值分别为：

以下对本发明的效果做进一步的验证；

选择船舶运动系统作为目标跟踪系统的典型例子验证发明的效果，对于船舶运动系统，首先确定目标跟踪系统数学模型，然后运用本发明中的方法进行matlab软件仿真，多方面比较asckfns算法与asckf算法的精确性与实时性。

(1)船舶运动数学模型:

选择系统状态向量为：

xk＝[x，x′，y，y′]^t；

式中，x为船舶x轴位移；x′为船舶x轴速度；y为船舶y轴位移；y′为船舶y轴速度。

由于需要验证asckfns算法对数学模型不精确的系统也有较好的控制性能，故设置了两种相差比较大的运动状态进行转换，选择船舶在二维平面内运动先为角速度恒定的圆周运动再为匀速直线运动：

式中，ω为角速度；t为离散系统采样时间；k为当前采样次数，k1为切换运动状态时刻已经经过的采样次数，n为设定的算法最大采样次数。

选择量测向量为：

zk＝[l，θ]^t；

式中，l为以雷达位置为原点的极坐标系内船舶位置的极径，θ为以雷达位置为原点的极坐标系内船舶位置的极角。

选择雷达为平面直角坐标系和极坐标系的共同的原点位置：

(2)为验证算法精确性进行matlab软件仿真；

选择系统噪声初值为：q0＝diag[0.251]；

量测噪声初值为：r0＝diag[12.25]；

选择协方差矩阵初值为：p0＝diag[1111]；

选择船舶位置初值为：x0＝[4101520010]^t；

令采样周期t＝0.2；角速度ω＝1；遗忘因子参数值b＝0.98；切换运动状态时刻已经经过的采样次数k1＝30；设定的算法最大采样次数n＝40，选择状态向量xk的第一个元素位移x作为横坐标、第三个元素位移y作为纵坐标，进行matlab软件仿真，可绘制直角坐标系内的目标运动轨迹图，得到asckfns算法、asckf算法与真实运动轨迹图。

图2中，黑点表示真实运动轨迹，连续曲线表示asckfns算法运动轨迹，间断曲线表示asckf算法运动轨迹。由图2可知，黑点大致都在连续曲线上，表明asckfns算法精确性较好，但不能明显看出asckfns算法、asckf算法的精确性的差异。为全面比较两者性能，本发明可以分别绘制状态向量xk四个元素的asckfns算法、asckf算法与真实值的对比图，如图3所示

从图3的(a)与(c)中可以看出，连续曲线波动比间断曲线稍小，表明asckfns算法比asckf算法在针对状态向量的位移x与位移y的滤波情况下精确性更高，但(b)与(d)中连续曲线与间断曲线重合，无法看出速度x’与速度y’的差别，为更准确区别asckfns算法与asckf算法的效果，本发明定义了均方根误差σ：

表示第j次采样时刻状态向量第i个元素的估计值，m为状态向量维数，此次仿真中，m＝4。均方根误差σ可衡量在前k次采样内状态向量xk四个元素的估计值与真实值的综合误差。进行matlab软件仿真，为跟踪算法长时间运行的效果，设定的算法最大采样次数增加为n＝200，为验证asckfns算法对噪声统计特性不准确的系统也有较好的控制性能，故在仿真中还可以对系统噪声与量测噪声进行改变，选择在第100次采样时刻将与分别扩大为与其他参数不变，可绘制asckfns算法、asckf算法状态向量均方根误差对比图，如图4所示；

图4中，连续曲线表示asckfns算法的状态向量均方根误差，间断曲线表示asckf算法的状态向量均方根误差，连续曲线的大部分采样点都在间断曲线下方，表明asckfns算法准确性效果较好。在第30次采样时，系统运动方程发生突变，但均方根误差仍较小，可以适用于数学模型不确定的系统中去。且在第100次采样时刻，虽然系统噪声、量测噪声突增为原来2倍，asckfns算法也很稳定，这是因为其噪声估计器中包含了遗忘因子，可以针对噪声环境产生自适应行为，对噪声统计特性不准确的情况也可以适用。

为验证算法实时性，本发明对asckfns算法与asckf算法在时间更新环节与噪声估计器环节中变化的公式的计算复杂度进行统计。

①在时间更新环节，假设状态向量为m维，量测向量为n维，忽略代入容积点求非线性函数值的时间、求雅克比矩阵的时间、求转置矩阵的时间。

asckf算法时间更新环节过程为：

sk-1|k-1＝chol(pk-1|k-1)；

xj，k|k-1＝f(xj，k-1|k-1)；

asckf算法求状态向量先验预测值所需计算量为1次矩阵分解运算、2m³+m次乘法运算、2m³+2m²-m次加法运算；asckf算法求误差协方差矩阵先验预测值的平方根所需计算量为3次矩阵分解运算、2m³+2m²+m次乘法运算、2m³+4m²-m次加法运算。

asckfns算法时间更新环节过程为：

sk|k-1＝chol(pk|k-1)；

asckfns算法求状态向量先验预测值所需计算量为m²次乘法运算、m²-m次加法运算；asckf算法求误差协方差矩阵先验预测值的平方根所需计算量为1次矩阵分解运算、2m³次乘法运算、2m³-m²次加法运算。

asckf算法的计算复杂度要大于asckfns算法，这是因为asckfns算法将非线性系统系统函数近似等效成线性系统之后，可以减少矩阵分解的次数，而且asckf算法中需要用2m个容积点来近似计算高斯加权积分，其时间更新运算时间与状态维数有密切联系。

②在噪声估计器环节，假设状态向量为m维，量测向量为n维，忽略求雅克比矩阵的时间、求转置矩阵的时间。

asckf算法噪声估计器环节过程为：

asckfns算法噪声估计器环节过程为：

当敛散判决成立时，则：

若敛散判决不成立时，则：

对比上述两种算法的噪声估计器环节，可看出无论敛散判决是否成立，asckf算法的计算复杂度要大于asckfns算法，这是因为asckfns算法进行了减项处理。

总的来说，asckfns算法的实时性比asckf算法的实时性要好。

应当理解的是，本说明书未详细阐述的部分均属于现有技术。

应当理解的是，上述针对较佳实施例的描述较为详细，并不能因此而认为是对本发明专利保护范围的限制，本领域的普通技术人员在本发明的启示下，在不脱离本发明权利要求所保护的范围情况下，还可以做出替换或变形，均落入本发明的保护范围之内，本发明的请求保护范围应以所附权利要求为准。