CN109115228B

CN109115228B - 一种基于加权最小二乘容积卡尔曼滤波的目标定位方法

Info

Publication number: CN109115228B
Application number: CN201811240267.2A
Authority: CN
Inventors: 鄢社锋; 徐立军; 石桂欣
Original assignee: Institute of Acoustics CAS
Current assignee: Zhejiang Wanghaichao Technology Co ltd
Priority date: 2018-10-23
Filing date: 2018-10-23
Publication date: 2020-08-18
Anticipated expiration: 2038-10-23
Also published as: CN109115228A

Abstract

本发明公开了一种基于加权最小二乘容积卡尔曼滤波的目标定位方法，所述方法包括：将最小二乘估计和容积卡尔曼滤波方法进行结合，借助于最小二乘估计将最新测量结果集成到每个容积粒子中，从而促进容积点从先验区域到高似然区域的移动；并对量测信息进行了扩维，从而计算每一时刻目标的状态和估计误差方差。本发明的定位方法具有实现简单，计算量小，满足水下定位系统低功耗的需求；而且有效提高了目标状态的估计精度。

Description

一种基于加权最小二乘容积卡尔曼滤波的目标定位方法

技术领域

本发明涉及目标定位和导航领域，具体涉及一种基于加权最小二乘容积卡尔曼滤波的目标定位方法。

背景技术

近年来，各国积极推进对海洋资源的探索。海底资源挖掘、渔业捕捞、水下机器人定位等相关研究也获得极大的关注。水下目标定位与跟踪技术作为水下作业的基础研究，无疑具有非常重要的地位。水下目标跟踪系统中，常常利用目标跟踪算法对距离、角度等测量信息进行处理进而得到水下目标的目标轨迹。现有的滤波跟踪算法包括卡尔曼滤波(Kalman Filter，KF)，扩展卡尔曼滤波(Extended Kalman Filter，EKF)无迹卡尔曼滤波(Unscented Kalman Filter，UKF)、容积卡尔曼滤波(Cubature Kalman Filter，CKF)、粒子滤波(Particle Filter，PF)等一系列经典算法。

PF算法虽然对非高斯非线性系统具有很高的估计精度，但是计算量巨大和易出现粒子匮乏现象的缺点使其受限于水下目标跟踪系统中的应用。KF是针对线性高斯系统的最优滤波器，但是实际中水下目标的运动轨迹肯定是非线性的，随着技术的发展，水下定位跟踪技术对估计精度的要求也越来越高，因此KF也远远不能满足人们的需求。EKF、UKF、CKF均是KF在非线性系统上的应用形式。EKF的主要核心通过对非线性函数的Taylor展开式进行一阶线性化截断并忽略高阶项,这要求非线性函数是近线性的。当系统是强非线性的时候，EKF算法的误差会急剧增加，不再适应人们的需求。UKF和CKF则分别对非线性函数进行二阶和三阶Taylor近似，相应地估计精度也逐渐变高，对强非线性系统的适应性也逐渐变强。

随着技术的发展，水下定位跟踪技术对估计精度的要求也越来越高。同时，相比无线中的目标定位跟踪系统，水下目标定位和跟踪系统又具有量测信息噪声大、信道不稳定、系统功耗受限等缺点，因此，寻找一种简单又能保证高精度的改进目标定位方法具有重大的意义。

发明内容

本发明的目的在于克服上述技术缺陷，提出了一种基于加权最小二乘容积卡尔曼滤波的目标定位方法。

本发明的技术方案为：

一种基于加权最小二乘容积卡尔曼滤波的目标定位方法，所述方法包括：将最小二乘估计和容积卡尔曼滤波方法进行结合，借助于最小二乘估计将最新测量结果集成到每个容积粒子中，从而促进容积点从先验区域到高似然区域的移动；并对量测信息进行了扩维，从而计算每一时刻目标的状态和估计误差方差。

作为上述方法的一种改进，所述方法具体包括：

步骤1)获取k-1时刻目标的状态

和估计误差方差P_k-1,k-1；以及k时刻的量测信息Z_k；

步骤2)k-1时刻目标的状态

和估计误差方差P_k-1,k-1生成m个容积粒子，引入k时刻的量测信息Z_k对每个容积粒子上使用最小二乘估计进行优化，得到优化后的容积粒子值，然后根据容积卡尔曼滤波算法得到k时刻目标的状态

和估计误差方差P_k,k；

步骤3)输出k时刻目标的状态

和估计误差方差P_k,k。

作为上述方法的一种改进，所述k-1时刻目标的状态

和估计误差方差P_k-1,k-1生成m个容积粒子，具体包括：

步骤2-1-1)对k-1时刻的目标的状态估计误差方差P_k-1,k-1进行分解得到：

式中S_k-1,k-1是对P_k-1,k-1做Cholesky分解得到的矩阵；

步骤2-1-2)计算每个cubature点

其中

n_x为目标状态变量的维数；

步骤2-1-3)计算更新后的cubature点

其中，f(·)为状态转移函数。

作为上述方法的一种改进，所述引入k时刻的量测信息Z_k对每个容积粒子上使用最小二乘估计进行优化，得到优化后的容积粒子值；具体包括：

步骤2-2-1)从每一个由步骤2-1-3)计算得出的容积点

中提取出对量测信息Z_k有贡献的元素，记为

求出

对应的雅克比矩阵

步骤2-2-2)计算优化后的值x^*；

若不考虑权重信息，则：

其中，I变为n_p×n_p的单位矩阵，n_p是

的长度，其中，h(·)为量测信息函数，λ的定义为：

若考虑权重信息，权重矩阵W是一个对角矩阵，对角线上的元素是各个量测误差值对应的权重；则：

步骤2-2-3)用步骤2-2-2)的x^*第二次更新

得到优化后的容积粒子值。

作为上述方法的一种改进，所述根据容积卡尔曼滤波算法得到k时刻目标的状态

和估计误差方差P_k,k，具体包括：

步骤2-3-1)根据步骤2-2-3)得到优化后的容积粒子值

计算一步预测状态

步骤2-3-2)计算一步预测误差方差P_k,k-1：

其中，Q_k-1是k-1时刻的测量系统噪声的协方差矩阵。

步骤2-3-3)对一步预测误差方差P_k,k-1进行分解：

其中S_k,k-1是对P_k,k-1做Cholesky分解得到的矩阵；

步骤2-3-4)计算cubature点

步骤2-3-5)计算通过量测方程的cubature点

其中，h(·)为量测信息函数；

步骤2-3-6)计算量测估计值

步骤2-3-7)计算更新后的量测误差方差

其中R_k是k时刻的量测噪声的协方差矩阵；

步骤2-3-8)计算协方差

步骤2-3-9)计算卡尔曼滤波增益K_k：

步骤2-3-10)计算更新后的k时刻的目标状态

步骤2-3-11)计算k时刻的目标状态

的估计误差方差P_k,k

作为上述方法的一种改进，所述步骤1)前还包括：

当k＝0时刻的初始化：结合先验知识给定目标的状态

和估计误差方差P_0,0、权重矩阵W；由量测信息的性质确定量测信息函数h(·)；设定状态转移函数f(·)。

本发明的优势在于：

1、本发明的定位方法具有实现简单，计算量小，满足水下定位系统低功耗的需求；

2、本发明的定位方法有效提高了目标状态的估计精度；

3、当初始值偏差很大时，本发明所提方法收敛更快。

附图说明

图1为本发明的目标定位方法的流程图；

图2为CKF与LS-CKF的估计轨迹对比图；

图3为CKF与LS-CKF的定位误差曲线；

图4为原始数据、CKF与LS-CKF的定位RMSE曲线对比图；

图5为CKF、LS-CKF与WLS-CKF的估计轨迹对比图；

图6为CKF、LS-CKF与WLS-CKF的估计误差曲线对比图；

图7为CKF、LS-CKF与WLS-CKF的RMSE对比图。

具体实施方式

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

本发明提出了最小二乘卡尔曼滤波器(LS-CKF)和加权最小二乘卡尔曼滤波器(WLS-CKF)。其中前者是后者在某些情况下的特例。该方法将最小二乘法和容积卡尔曼滤波算法结合起来，借助于最小二乘估计将最新测量有效地集成到每个容积粒子中，从而促进容积点从先验区域到高似然区域的移动。此外，为了克服该方法求解过程中可能存在的不可逆问题，对量测信息进行了扩维，完善了滤波器的求解方程。新算法具有操作简单、估计精度高的优势，仿真实验也证明了这一点。

1、状态系统模型

考虑用如下状态空间模型描述的动态系统

X_k＝f(X_k-1)+w_k-1， (1)

Z_k＝h(X_k)+v_k， (2)

式中，k为离散时间，系统在时刻k的状态为

为对应状态的观测信号，

为输入的白噪声，

为观测噪声。

其中，式(1)为状态方程，式(2)为观测方程，f(·)为状态转移函数，h(·)为量测信息函数。

2、标准容积卡尔曼滤波

CKF算法的核心是球面-径向容积规则，它提供了一组与状态向量维度成线性比例的容积点，这使得非线性贝叶斯滤波器中遇到的多元积分可以进行数值计算。因此，CKF可以为高维非线性滤波问题提供系统解决方案。

容积卡尔曼滤波算法的基本步骤如下所示。

假设时刻k-1的估计方差P_k-1,k-1是已知的，对其分解得到：

计算cubature点

其中

n_x为状态变量的维数，如果n_x＝2，则[1]_i的四个元素可以对称地表示为

计算更新后的cubature点

计算一步预测状态：

计算一步预测误差方差：

对一步预测误差方差进行分解：

计算cubature点

计算通过量测方程的cubature点

计算量测估计值：

计算更新后的量测误差方差：

计算协方差：

计算卡尔曼滤波增益：

计算更新后的状态

计算相应的估计误差方差

3、最小二乘-容积卡尔曼滤波(LS-CKF)

为了提高方法的精度，本发明将最小二乘引入时间更新过程。利用最小二乘为指导每个容积点的采样。因为在时间更新过程中已经“吸纳”了最新的观测信息Z_k，所以最终产生的容积点会更靠近真实值，一步预测的状态向量精度也会得到提高。

根据式(3)-(5)可知

中只含有先验信息，并没有利用最新的量测信息。现在，在每个容积点上使用最小二乘算法进行优化。为表述方便，k时刻目标的状态向量为x，某个容积点的估计值记为

(即，

是x的近似值)，

经过最小二乘法优化后的值为x^*，该采样k时刻的量测信息仍记为Z_k。首先构造代价函数：

J＝[Z_k-h(x)]^T[Z_k-h(x)]。 (17)

所以

将h(x)在

处进行一阶泰勒展开，可得

式中H_k是一个雅克比矩阵，

于是可得

式中

将代价函数J对x求导并化简可得

如果

是可逆的，那么有

下面要考虑两个问题。

一、

不可逆的情况

已知x和Z_k分别为n_x维和n_z维向量。如果x的维数比较高，而Z_k的维数比较低，则有可能

式中rank[·]表示求给定矩阵的秩。为了使

可逆，我们需要提高H_k的秩。状态向量的维数是固定的，不可随便减少，但是量测信息可以扩维。构造一个新的量测量：

相应地，式(22)的通用表达式为

式中I是n_x×n_x的单位矩阵，λ的定义如下

二、量测信息只由状态向量中的部分元素决定

如果Z_k的取值只由状态向量中的部分元素决定，那么H_k中会出现很多0元素。以基于测距信息的目标跟踪系统为例，假设

Z_k是目标到各个节点的距离，那么

的值只与x和y的估计值相关，这种情况下，使用式(25)计算得出的x^*只是被修正了前两个元素的取值，速度信息和加速度信息并未变化。H_k的第3到第6列全部为0元素，即

式中

是H_k中前两列非零向量组成的n_z×2维矩阵，0是一个n_z×4维的零矩阵。当n_z≥2时，由雅克比矩阵的定义以及相关求导知识可得

不需要对Z_k扩维，直接将(22)中的H_k替换为

替换为

就可以求出优化后的估计值x^*。

易得出通用表达式的最精简形式如下：

注意式中I变为n_p×n_p的单位矩阵，n_p是

的长度，λ的定义也稍有变化

值得注意的是，使用

替换

H_k以后，

就变成了一个非常宽泛的条件，实际中很容易满足。仍然以基于测距信息的目标跟踪系统为例，只要n_z≥n_p，该条件就可以满足。由此，便去除了冗余计算量，减轻了系统的功耗负担。

三、量测信息中的各元素量级相差比较大

如果Z_k中各个元素的量级相差比较大，比如既有距离又有方位角，根据前文中代价函数的定义，J的取值主要由误差量级大的元素(距离)决定，误差量级小的元素(方位角)几乎不影响，除非前者与后者的误差量级是接近的。

因此，可以将代价函数改写为J＝{W[Z_k-h(x)]}^T{W[Z_k-h(x)]}, (31)

其中W是加权矩阵，同时也是一个对角矩阵，对角线上的元素是各个量测误差值对应的权重。考虑了权重信息W后，式(27)改写如下：

为了区别于前面不考虑加权矩阵W的LS-CKF方法，将更新后的方法称为加权最小二乘-容积卡尔曼滤波(WLS-CKF)方法。事实上，LS-CKF方法是WLS-CKF方法在W＝I时的特例。

如图1所示，下面给出基于本发明的目标定位方法的具体步骤：

步骤1)初始化，k＝0。设定参数P、R、Q、f(·)，以及结合先验知识给定目标坐标的初值X₀、W。

步骤2)传感器采集目标信息，如目标相对于传感器的方位角、距离等，采集到的信息称为量测信息Z，k时刻的量测信息记为Z_k。

步骤3)根据量测信息的性质确定观测信息函数h(·)。

步骤4)由公式(3)-(5)计算出

的初值；

取

结合实际情况确定

求出

对应的

结合Z_k，由式(29)或式(32)者求出优化后的值x^*；

更新

量测更新阶段不变，参照式(6)-(16)。

步骤5)

和P_k,k是跟踪系统在k时刻的输出。

步骤6)如果对目标的跟踪没有结束，则重复步骤2)-步骤5)，继续向下处理；否则输出结果，整个跟踪阶段结束。

可以看出，本发明的方法并未增加很多计算负担，标准CKF方法的计算效率高优点得以保留。

4、性能分析

下面采用蒙特-卡洛实验来分析本发明的LS-CKF方法和WLS-CKF方法的性能，并与原CKF方法进行比较。首先考虑k时刻的状态向量为X_k＝[x,y,v_x,v_y,a_x,a_y]^T,根据式(1)和式(2)建立状态方程和观测方程如下：

X_k＝ΦX_k-1+w_k， (33)

Y_k＝h(X_k,v_k)＝dist(X_k,{A,B,C})+v_k， (34)

记T为采样间隔，式(33)中的状态转移矩阵为

w_k和v_k分别是均值为零方差为Q和R的高斯白噪声；dist(X_k,{A,B,C})表示X_k确定的坐标离节点{A,B,C}的距离。

目标从原点处开始先做匀速直线(CV)运动，速度为v_x＝5m/s，v_y＝3m/s，5s后加速度为a_x＝-0.1m/s²，a_y＝0.1m/s²并持续到55s，55s到105s加速度为

a_y＝-0.1m/s²，105s到110s做CV运动，速度为上一阶段结束时(105s)目标的速度。给定三个节点的坐标分别为[0m,500m]、[250m,100m]、[300m,400m]。量测信息为三个节点测得目标离自己的距离，量测信息协方差取为R＝αI＝diag[25,25,25]，α其中为量测噪声控制因子。使用CA模型对目标进行状态估计，初始状态向量为X₀＝[100,50,0,0,0,0]^T，利用标准CKF方法和本发明的方法分别对该被动目标的位置信息进行跟踪和估计。假设过程噪声方差为Q＝diag[1,1,0.1,0.1,0.01,0.01]，初始协方差矩阵为单位矩阵，即

其中n是状态向量的维数，此处n＝6。本实验中的量测向量均为距离，各个距离误差量级是相同的，因此计算代价函数时不对量测值进行加权，取W＝I。

图2给出了CKF和LS-CK方法轨迹估计结果与真实轨迹的对比。图3中这两种方法在各采样点的定位误差曲线。显然，本发明所提出的LS-CKF方法具有收敛快、精度高的特点。本实验中实际初始位置与给定初值相差很大，标准CKF方法经过四次计算，在第五个采样时刻才使得定位误差小于40m，第六个采样时刻误差约为20m，前10个采样时刻的定位误差均大于10m。而本发明的方法在第二个采样时刻的定位误差就低于40m，第三个采样时刻误差低于20m，从第四个采样时刻开始，定位误差均小于10m。

下面，将初始状态向量改为更接近真实值的X₀＝[0,0,0,0,0,0]^T，量测噪声控制因子α从0.1变化到100，其他条件不变。进行蒙特卡洛仿真实验50次，统计各跟踪方法的均方根误差(RMSE)，如图4所示。图4中“原始数据”表示不使用任何滤波方法，用几何定位解算目标位置的误差。可以看出，虽然随着量测噪声的增加，各个方法的RMSE都会增加，但是LS-CKF的RMSE始终是最低的。

下面进行另一组仿真实验。将模型的量测信息替换为目标到A节点的距离和目标相对于A节点的方位(弧度)，即

初始状态向量为X₀＝[4,-100,5,3,0,0]^T，量测信息协方差取为R＝α·diag[1,0.0001]，α＝10。计算代价函数时量测值加权矩阵取W＝diag[1,100]。仿真结果如图5和图6所示。

图5给出了CKF、LS-CKF和WLS-CKF方法轨迹估计结果与真实轨迹的对比。图6中这几种方法在各采样点的定位误差曲线。显然，本发明所提出的LS-CKF和WLS-CKF方法均具有收敛快、精度高的特点。

下面，将初始状态向量改为更接近真实值的X₀＝[4,-17,5,3,0,0]^T，量测噪声控制因子α从1变化到30，其他条件不变。进行蒙特卡洛仿真实验50次，统计各跟踪方法的均方根误差(RMSE)，如图7所示。可以看出，虽然随着量测噪声的增加，各个方法的RMSE都会增加，但是LS-CKF和WLS-CKF的RMSE始终低于CKF的RMSE，且WLS-CKF的RMSE一直都低于LS-CKF。以上结果验证了本发明方法的优异性能。

最后所应说明的是，以上实施例仅用以说明本发明的技术方案而非限制。尽管参照实施例对本发明进行了详细说明，本领域的普通技术人员应当理解，对本发明的技术方案进行修改或者等同替换，都不脱离本发明技术方案的精神和范围，其均应涵盖在本发明的权利要求范围当中。