CN103278813B - 一种基于高阶无迹卡尔曼滤波的状态估计方法 - Google Patents
一种基于高阶无迹卡尔曼滤波的状态估计方法 Download PDFInfo
- Publication number
- CN103278813B CN103278813B CN201310158729.7A CN201310158729A CN103278813B CN 103278813 B CN103278813 B CN 103278813B CN 201310158729 A CN201310158729 A CN 201310158729A CN 103278813 B CN103278813 B CN 103278813B
- Authority
- CN
- China
- Prior art keywords
- kappa
- sigma
- formula
- state
- measurement
- Prior art date
- Legal status (The legal status is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the status listed.)
- Expired - Fee Related
Links
Landscapes
- Complex Calculations (AREA)
- Management, Administration, Business Operations System, And Electronic Commerce (AREA)
Abstract
一种基于高阶无迹卡尔曼滤波的状态估计方法,本发明具体涉及一种基于高阶无迹卡尔曼滤波的状态估计方法。本发明采用高阶无迹卡尔曼滤波器完成目标跟踪过程中的状态估计任务。在目标跟踪过程中,建立目标跟踪的状态方程和量测方程;采用高阶无迹变换获得目标跟踪滤波器所需的sigma点,并计算其权值;通过迭代sigma点及其权值获取对状态的估计,实现对目标的实时跟踪。其跟踪精度高于现有的基于其它滤波器的目标跟踪方法。通过选用合适的性能参数κ,能够进一步提升本发明提出的高阶UKF目标跟踪方法精度,实现对目标的高精度实时跟踪。本发明应用于目标跟踪技术领域。
Description
技术领域
本发明属于非线性滤波及目标跟踪技术领域,具体涉及一种基于高阶无迹卡尔曼滤波的状态估计方法。
背景技术
在目标定位跟踪过程中,观测站通常仅能够获得包含噪声的目标方位信息,例如利用雷达对空中目标进行观测时,雷达能够获得空中目标相对自身的方位角,但该观测含有噪声。在假设目标是匀速飞行的情况下,需要估计目标位置从而实现对目标的跟踪。此时观测方程中雷达的方位观测量是待估计目标位置参数的非线性函数,不能直接利用线性滤波方法获得目标的运动状态,其本质为非线性滤波问题,是目标跟踪、机器视觉以及传感器网络定位等研究领域的共同难题。为了解决这一难题,人们首先试图使用线性化方法,从而产生了扩展卡尔曼滤波器(Extended Kalman Filter,EKF)。但是EKF在目标定位跟踪应用中存在两个很大的缺点,首先,当系统非线性很强时,围绕状态预测估值或状态估值的一阶泰勒展开方法往往存在很大的截断误差,使得跟踪效果不理想,甚至出现滤波发散的现象;其次一阶EKF需要计算Jacobi矩阵,二阶EKF还需要计算Hessian矩阵,计算量大,对处理器要求高。
为解决非线性滤波估计问题,研究人员提出一种新的用于解决非线性滤波问题的方法,它是基于这样一种考虑:近似一种高斯分布要比近似任何一种非线性函数容易的多。人们将这样一种滤波器称为无迹卡尔曼滤波器(Unscented Kalman Filter,UKF)。研究表明UKF给出的估计结果比EKF更准确,它能达到二阶EKF估计精度,但是计算量却远远小于二阶EKF。
目前人们在目标跟踪领域内应用的UKF都是二阶UKF方法,对于高斯非线性系统,二阶UKF的估计精度只能达到非线性函数的三次泰勒展开项,精度有限,对于一些对估计精度要求较高的场合,二阶UKF不再适用。
发明内容
本发明为了解决现有技术在目标定位跟踪的过程中,存在很大的截断误差,使得跟踪效果不理想,从而提出了一种基于高阶无迹卡尔曼滤波的状态估计方法。
一种基于高阶无迹卡尔曼滤波的状态估计方法包括下述步骤:
步骤一:建立描述目标跟踪系统的状态方程和量测方程;
步骤二:根据目标跟踪系统的状态的维数选择高阶UKF性能参数κ;
步骤三:一步状态预测,通过高阶无迹变换获得用于一步状态预测的sigma点,并将所述的sigma点经状态方程传播得到一步状态预测的样本点,对一步状态预测的样本点进行加权计算得到一步状态预测估计和一步状态预测估计误差协方差矩阵;
步骤四:一步量测预测,通过高阶无迹变换获得用于一步量测预测的sigma点,并将所述的sigma点经量测方程传播得到一步量测预测的样本点,对一步量测预测的样本点进行加权计算得到一步量测预测估计和一步量测估计误差协方差矩阵,对一步状态预测的样本点和一步量测预测的样本点进行加权计算得到状态与量测的互协方差矩阵;
步骤五:状态滤波更新,根据步骤三获得的一步状态预测估计和一步状态预测估计误差协方差矩阵,和步骤四获得的一步量测预测估计、一步量测估计误差协方差矩阵和状态与量测的互协方差矩阵,完成状态的滤波更新,得到状态的线性最小方差估计和估计误差协方差矩阵,完成目标跟踪的状态估计任务。
步骤一所述的建立描述目标跟踪系统的状态方程和量测方程为:
其中,状态方程为xk=f(xk-1)+nk-1,量测方程为zk=h(xk)+vk,k-1表示第k-1步,k表示第k步,xk为第k步的n维跟踪目标参数状态向量,zk为第k+1步的m维跟踪目标参数的量测向量,f(·)和h(·)均为已知的非线性函数,nk-1为第k-1步n维随机系统噪声,vk为第k步m维随机量测噪声,随机系统噪声向量nk-1服从均值为0、方差为Qk-1的高斯分布,Qk-1表示第k-1步系统噪声的方差阵,随机量测噪声向量vk服从均值为0、方差为Rk的高斯分布,Rk表示第k步量测噪声的方差阵,且nk-1与vk不相关。
步骤二所述的根据目标跟踪系统的状态的维数选择高阶UKF性能参数κ的具体过程为:
根据性能参数κ的一元二次方程:
(n-1)κ2+(2n2-14n)κ+n3-13n2+60n-60=0
求解性能参数κ,当系统的状态的维数n=2时,性能参数κ=0.835;
当系统的状态的维数n=3时,性能参数κ=1.417;
当系统的状态的维数n=1或n≥4时,性能参数κ的一元二次方程无实数解,即不存在理论最优的性能参数κ的值,设定κ=2。
步骤三所述的对一步状态预测的样本点进行加权计算得到一步状态预测估计和一步状态预测估计误差协方差矩阵的具体过程为:
第k-1步待估计跟踪目标参数构成的状态向量xk-1的后验密度函数k-1|k-1表示采用观测集合[z0…zk-1]对相应的参数进行估计,根据公式(2)分解误差协方差Pk-1|k-1获得矩阵Sk-1|k-1:
根据公式(3)和(4)分别计算第一类sigma点X00,k-1|k-1和该第一类sigma点的权值w0:
根据公式(5)和(6)计算具有相同权值的2n个第二类sigma点和其权值w1:
其中,i1=1,2,...,n,表示第i1个坐标为1的单位列向量,
根据公式(7)和(8)计算具有相同权值的2n(n-1)个第三类sigma点 和该第三类sigma点的权值w2:
其中,i2=1,2,...,n(n-1)/2,
其中,el=[0,0,....1,...0]T表示第l个坐标为1的单位列向量;ep=[0,0,....1,...0]T表示第p个坐标为1的单位列向量,
根据公式(11)计算状态方程传播sigma点:
根据公式(12)估计第k步的一步状态预测估计
根据公式(13)估计第k步的一步状态预测估计误差协方差矩阵Pk|k-1:
步骤四所述的对一步量测预测的样本点进行加权计算得到一步量测预测估计和一步量测估计误差协方差矩阵,对一步状态预测的样本点和一步量测预测的样本点进行加权计算得到状态与量测的互协方差矩阵的具体步骤为:
根据公式(14)分解一步状态预测估计误差协方差矩阵Pk|k-1获得Sk|k-1:
根据公式(15)和(16)计算第一类sigma点X00,k|k-1和该第一类sigma点X00,k|k-1的权值w10:
根据公式(17)和(18)计算具有相同权值的2n个第二类sigma点和该第二类sigma点的权值w11:
根据公式(19)和(20)计算具有相同权值的2n(n-1)个第三类sigma点 和该第三类sigma点的权值w12:
根据公式(21)计算量测方程传播sigma点:
根据公式(22)估计第k步的量测的一步量测预测估计
根据公式(23)估计第k步量测的一步估计误差协方差矩阵Pzz,k|k-1:
根据公式(24)估计状态与量测的互协方差矩阵:
步骤五所述的根据步骤三获得的一步状态预测估计和一步状态预测估计误差协方差矩阵,和步骤四获得的一步量测预测估计、一步量测估计误差协方差矩阵和状态与量测的互协方差矩阵,完成状态的滤波更新,得到状态的线性最小方差估计和估计误差协方差矩阵的具体过程为:
根据公式(25)估计高阶UKF的滤波增益Wk:
根据公式(26)计算第k步状态滤波估计
根据公式(27)计算第k步状态估计误差协方差矩阵Pk|k计算如下:
本发明的优点在于:
1、通过建立目标跟踪的数学模型,并引入一个性能参数κ给出了一种基于高阶UKF的目标跟踪方法。实现了截断误差小的目的。
2、通过理论分析和实际运行经验,得到在二维和三维情况下,当κ分别取κ=0.835和κ=1.417时,高阶UKF可以获得比现有的二阶UKF、二阶CKF及高阶CKF方法更优的性能,达到了提高目标跟踪的精度的目的。
附图说明
图1是本发明所述的一种基于高阶无迹卡尔曼滤波的状态估计方法程图;
图2为图1的详细方法流程图;
图3是本发明提供的方法与基于二阶UKF、二阶CKF及高阶CKF的目标跟踪应用中对目标x坐标轴方向位置估计的均方误差曲线;图中A表示二阶CKF对目标x坐标轴方向位置估计的均方误差曲线,B表示二阶UKF对目标x坐标轴方向位置估计的均方误差曲线,C表示高阶CKF对目标x坐标轴方向位置估计的均方误差曲线,D表示本发明提供的方法对目标x坐标轴方向位置估计的均方误差曲线;
图4是本发明提供的方法与基于二阶UKF、二阶CKF及高阶CKF的目标跟踪应用中对目标y坐标轴方向位置估计的均方误差曲线。
具体实施方式
具体实施方式一、结合图1具体说明本实施方式,本实施方式所述的一种基于高阶无迹卡尔曼滤波的状态估计方法包括下述步骤:
步骤一:建立描述目标跟踪系统的状态方程和量测方程;
步骤二:根据目标跟踪系统的状态的维数选择高阶UKF性能参数κ;
步骤三:一步状态预测,通过高阶无迹变换(Unscented Transformation,UT)获得用于一步状态预测的sigma点,并将所述的sigma点经状态方程传播得到一步状态预测的样本点,对一步状态预测的样本点进行加权计算得到一步状态预测估计和一步状态预测估计误差协方差矩阵;
步骤四:一步量测预测,通过高阶无迹变换获得用于一步量测预测的sigma点,并将所述的sigma点经量测方程传播得到一步量测预测的样本点,对一步量测预测的样本点进行加权计算得到一步量测预测估计和一步量测估计误差协方差矩阵,对一步状态预测的样本点和一步量测预测的样本点进行加权计算得到状态与量测的互协方差矩阵;
步骤五:状态滤波更新,根据步骤三获得的一步状态预测估计和一步状态预测估计误差协方差矩阵,和步骤四获得的一步量测预测估计、一步量测估计误差协方差矩阵和状态与量测的互协方差矩阵,完成状态的滤波更新,得到状态的线性最小方差估计和估计误差协方差矩阵,完成目标跟踪的状态估计任务。
具体实施方式二、本实施方式与具体实施方式一所述的一种基于高阶无迹卡尔曼滤波的状态估计方法的区别在于,步骤一所述的建立描述目标跟踪系统的状态方程和量测方程为:
其中,状态方程为xk=f(xk-1)+nk-1,量测方程为zk=h(xk)+vk,k-1表示第k-1步,k表示第k步,xk为第k步的n维跟踪目标参数状态向量,zk为第k+1步的m维跟踪目标参数的量测向量,f(·)和h(·)均为已知的非线性函数,nk-1为第k-1步n维随机系统噪声,vk为第k步m维随机量测噪声,随机系统噪声向量nk-1服从均值为0、方差为Qk-1的高斯分布,Qk-1表示第k-1步系统噪声的方差阵,随机量测噪声向量vk服从均值为0,方差为Rk的高斯分布,Rk表示第k步量测噪声的方差阵,且nk-1与vk不相关。
具体实施方式三、结合图2具体说明本实施方式,本实施方式与具体实施方式一或二所述的一种基于高阶无迹卡尔曼滤波的状态估计方法的区别在于,步骤二所述的根据目标跟踪系统的状态的维数选择高阶UKF性能参数κ的具体过程为:
获取性能参数κ的最优值时,sigma点必须能捕获到先验随机向量4阶矩信息从而得到性能参数κ的一元二次方程:
(n-1)κ2+(2n2-14n)κ+n3-13n2+60n-60=0
求解性能参数κ,当系统的状态的维数n=2时,性能参数κ=0.835;当系统的状态的维数n=3时,性能参数κ=1.417;此时本发明提供的方法可以获得比现有的二阶UKF、二阶CKF及高阶CKF方法更优的性能;
当系统的状态的维数n=1或n≥4时,性能参数κ的一元二次方程无实数解,即不存在理论最优的性能参数κ的值,根据无迹变换的数值稳定角度考虑,设定κ=2,以获得跟踪稳定性。
具体实施方式四、本实施方式与具体实施方式三所述的一种基于高阶无迹卡尔曼滤波的状态估计方法的区别在于,步骤三所述的对一步状态预测的样本点进行加权计算得到一步状态预测估计和一步状态预测估计误差协方差矩阵的具体过程为:
第k-1步待估计跟踪目标参数构成的状态向量xk-1的后验密度函数k-1|k-1表示采用观测集合[z0…zk-1]对相应的参数进行估计,根据公式(2)分解误差协方差Pk-1|k-1获得矩阵Sk-1|k-1:
根据公式(3)和(4)分别计算第一类sigma点X00,k-1|k-1和该第一类sigma点的权值w0:
根据公式(5)和(6)计算具有相同权值的2n个第二类sigma点和其权值w1:
其中,i1=1,2,...,n,表示第i1个坐标为1的单位列向量,
根据公式(7)和(8)计算具有相同权值的2n(n-1)个第三类sigma点 和该第三类sigma点的权值w2:
其中,i2=1,2,...,n(n-1)/2,
其中,el=[0,0,....1,...0]T表示第l个坐标为1的单位列向量;ep=[0,0,....1,...0]T表示第p个坐标为1的单位列向量,
根据公式(11)计算状态方程传播sigma点:
根据公式(12)估计第k步的一步状态预测估计
根据公式(13)估计第k步的一步状态预测估计误差协方差矩阵Pk|k-1:
具体实施方式五、本实施方式与具体实施方式四所述的一种基于高阶无迹卡尔曼滤波的状态估计方法的区别在于,步骤四所述的对一步量测预测的样本点进行加权计算得到一步量测预测估计和一步量测估计误差协方差矩阵,对一步状态预测的样本点和一步量测预测的样本点进行加权计算得到状态与量测的互协方差矩阵的具体步骤为:
根据公式(14)分解一步状态预测估计误差协方差矩阵Pk|k-1获得Sk|k-1:
根据公式(15)和(16)计算第一类sigma点X00,k|k-1和该第一类sigma点X00,k|k-1的权值w10:
根据公式(17)和(18)计算具有相同权值的2n个第二类sigma点和该第二类sigma点的权值w11:
根据公式(19)和(20)计算具有相同权值的2n(n-1)个第三类sigma点 和该第三类sigma点的权值w12:
根据公式(21)计算量测方程传播sigma点:
根据公式(22)估计第k步的量测的一步量测预测估计
根据公式(23)估计第k步量测的一步估计误差协方差矩阵Pzz,k|k-1:
根据公式(24)估计状态与量测的互协方差矩阵:
具体实施方式六、本实施方式与具体实施方式五所述的一种基于高阶无迹卡尔曼滤波的状态估计方法的区别在于,步骤五所述的根据步骤三获得的一步状态预测估计和一步状态预测估计误差协方差矩阵,和步骤四获得的一步量测预测估计、一步量测估计误差协方差矩阵和状态与量测的互协方差矩阵,完成状态的滤波更新,得到状态的线性最小方差估计和估计误差协方差矩阵的具体过程为:
根据公式(25)估计高阶UKF的滤波增益Wk:
根据公式(26)计算第k步状态滤波估计
根据公式(27)计算第k步状态估计误差协方差矩阵Pk|k计算如下:
此时通过式(26)可以获得的第k步状态滤波估计,实现对目标的跟踪。式(26)给出的第k步状态滤波估计和式(27)给出的第k步状态估计误差协方差矩阵Pk|k将用于下一时刻目标位置参数的估计。
实施例:在目标跟踪过程中,观测站能够获得包含噪声的目标方位信息,而方位信息与待估计的目标位置信息间关系为非线性函数,造成了观测方程的非线性,通常应用EKF、二阶UKF、二阶CKF或高阶CKF非线性滤波方法来获得目标的运动状态。但在对目标定位跟踪精度要求较高的场合,上述滤波方法不能满足要求。本发明提供的方法比现有的方法具备更高的估计精度,能够提高目标跟踪的精度。下面以具体实施例子来说明本发明的优越性。具体如下:
步骤一:依据纯方位目标跟踪问题,建立如下描述目标跟踪系统的状态方程和观测方程:
其中,第k步的状态方程xk=[x1,k x2,k]T=[x y]T,表示x轴和y轴平面内(笛卡尔坐标系)目标的位置; 矩阵 表征了目标x方向速度为0.9/秒,y方向速度为1米/秒。随机系统噪声向量nk-1服从均值为0,方差为Qk-1的高斯分布, 随机量测噪声向量vk服从均值为0,方差为Rk的高斯分布,Rk=0.025rad2,初始真实状态值x0=[20m5m]T,初始协方差阵P0=[0.1m20m2;0m20.1m2]T,P0表征了目标初始位置的不确定性。
步骤二:根据系统状态的维数选择合适的高阶UKF性能参数κ,纯方位跟踪系统状态维数为n=2,因此取最优的κ值,κ=0.835。
步骤三:第k步的一步状态预测,应用高阶无迹变换获得用于状态一步预测的sigma点,并将这些sigma点经状态方程传播得到一步状态预测的样本点,加权计算得到的一步状态预测估计和一步状态预测估计误差协方差矩阵。
具体为,假设k-1步后验密度函数已知,通过Cholesky分解,根据公式(2)误差协方差Pk-1|k-1,
根据根据公式(3)和(4)分别计算第一类sigma点X00,k-1|k-1和该第一类sigma点的权值w0,得到:w0=0.416
根据公式(5)和(6)计算具有相同权值的2n个第二类sigma点和其权值w1,其中i1=1,2
获得:
w1=0.022
根据公式(7)和(8)计算具有相同权值的2n(n-1)个第三类sigma点 和该第三类sigma点的权值w2,
获得:
w2=0.124
其中,i2=1,在公式(9)和(10)中p<l,l,p=1,2
根据公式(11)计算状态方程传播sigma点,得到:
根据公式(12)估计第k步的一步状态预测估计得到:
根据公式(13)估计第k步的一步状态预测估计误差协方差矩阵Pk|k-1,得到:
步骤四:一步量测预测,应用高阶UT变换获得用于量测一步预测的sigma点,并将这些sigma点经量测方程传播得到量测一步预测的样本点,加权计算得到量测的一步预测估计和一步估计误差协方差矩阵,以及状态与量测的互协方差矩阵。
具体为,根据公式(14)分解一步状态预测估计误差协方差矩阵Pk|k-1获得Sk|k-1,根据公式(15)和(16)计算第一类sigma点X00,k|k-1和该第一类sigma点X00,k|k-1的权值w10,获得:
w10=0.416
根据公式(17)和(18)计算具有相同权值的2n个第二类sigma点和该第二类sigma点的权值w11,获得:
w11=0.022
根据公式(19)和(20)计算具有相同权值的2n(n-1)个第三类sigma点 和该第三类sigma点的权值w12,获得:
w12=0.124
根据公式(21)计算量测方程传播sigma点,得到:
根据公式(22)估计第k步的量测的一步量测预测估计获得:
根据公式(23)估计第k步量测的一步估计误差协方差矩阵Pzz,k|k-1,获得:
根据公式(24)估计状态与量测的互协方差矩阵,获得:
步骤五:状态滤波更新,应用前面步骤得到的状态和量测一步预测参数,完成状态的滤波更新,得到状态的线性最小方差估计和估计误差协方差矩阵,完成纯方位跟踪的目标位置估计任务。
具体为,根据公式(25)估计高阶UKF的滤波增益Wk,根据公式(26)计算第k步状态滤波估计根据公式(27)计算第k步状态估计误差协方差矩阵Pk|k,
此时,即为目标的位置估计,Pk|k为目标的位置估计误差协方差矩阵。和Pk|k将用于下一时刻对目标的位置和位置误差协方差矩阵的计算中。
实施过程:仿真过程中采用如下定义的均方误差性能指标,比较各种滤波方法:
其中N为Monte Carlo次数。对目标位置估计的均方误差值越小则表征目标跟踪精度越高,效果越好。
仿真时间100秒,在相同的仿真条件下采用二阶UKF、二阶CKF、高阶CKF和本发明提出的高阶UKF方法,在本发明方法中,参数κ取0.835。所有的方法均进行500次MonteCarlo仿真,以进行比较。
实施效果:图3给出了纯方位目标跟踪过程中,本发明提供的方法与二阶UKF、二阶CKF、高阶CKF方法对x方向位置估计的均方误差曲线;图4给出了纯方位目标跟踪过程中,本发明提供的方法与二阶UKF、二阶CKF、高阶CKF对y方向位置估计的均方误差曲线。在图3和图4中,曲线A代表二阶UKF状态估计值的均方误差曲线,曲线B代表二阶CKF状态估计值的均方误差曲线,曲线C代表高阶CKF状态估计值的均方误差曲线,曲线D代表本发明提供方法状态估计值的均方误差曲线,状态估计值的均方误差越小代表估计精度越高,性能越好。从图3和图4可以看出,二阶UKF性能优于二阶CKF,高阶CKF性能优于二阶UKF,所有的滤波方法中本发明提出的高阶UKF(κ=0.835)具备最优的性能。
从以上实施例不难看出,相对于现有的目标跟踪方法,本发明提供方法能够获得更高的精度,获取对目标更准确的跟踪。
Claims (1)
1.一种基于高阶无迹卡尔曼滤波的状态估计方法,其特征在于:它包括下述步骤:
步骤一:建立描述目标跟踪系统的状态方程和量测方程,方程为:
其中,状态方程为xk=f(xk-1)+nk-1,量测方程为zk=h(xk)+vk,k-1表示第k-1步,k表示第k步,xk为第k步的n维跟踪目标参数状态向量,zk为第k+1步的m维跟踪目标参数的量测向量,f(·)和h(·)均为已知的非线性函数,nk-1为第k-1步n维随机系统噪声,vk为第k步m维随机量测噪声,随机系统噪声向量nk-1服从均值为0,方差为Qk-1的高斯分布,Qk-1表示第k-1步系统噪声的方差阵,随机量测噪声向量vk服从均值为0、方差为Rk的高斯分布,Rk表示第k步量测噪声的方差阵,且nk-1与vk不相关;
步骤二:根据目标跟踪系统的状态的维数选择高阶UKF性能参数κ,具体过程为:
根据性能参数κ的一元二次方程:
(n-1)κ2+(2n2-14n)κ+n3-13n2+60n-60=0
求解性能参数κ,当系统的状态的维数n=2时,性能参数κ=0.835;
当系统的状态的维数n=3时,性能参数κ=1.417;
当系统的状态的维数n=1或n≥4时,性能参数κ的一元二次方程无实数解,即不存在理论最优的性能参数κ的值,设定κ=2;
步骤三:一步状态预测,通过高阶无迹变换获得用于一步状态预测的sigma点,并将所述的sigma点经状态方程传播得到一步状态预测的样本点,对一步状态预测的样本点进行加权计算得到一步状态预测估计和一步状态预测估计误差协方差矩阵,具体过程为:
第k-1步待估计跟踪目标参数构成的状态向量xk-1的后验密度函数k-1|k-1表示采用观测集合[z0…zk-1]对相应的参数进行估计,根据公式(2)分解误差协方差Pk-1|k-1获得矩阵Sk-1|k-1:
根据公式(3)和(4)分别计算第一类sigma点X00,k-1|k-1和该第一类sigma点的权值w0:
根据公式(5)和(6)计算具有相同权值的2n个第二类sigma点和其权值w1:
其中,i1=1,2,...,n,表示第i1个坐标为1的单位列向量,
根据公式(7)和(8)计算具有相同权值的2n(n-1)个第三类sigma点 和该第三类sigma点的权值w2:
其中,i2=1,2,...,n(n-1)/2,
其中,el=[0,0,....1,...0]T表示第l个坐标为1的单位列向量;ep=[0,0,....1,...0]T表示第p个坐标为1的单位列向量,
根据公式(11)计算状态方程传播sigma点:
根据公式(12)估计第k步的一步状态预测估计
根据公式(13)估计第k步的一步状态预测估计误差协方差矩阵Pk|k-1:
步骤四:一步量测预测,通过高阶无迹变换获得用于一步量测预测的sigma点,并将所述的sigma点经量测方程传播得到一步量测预测的样本点,对一步量测预测的样本点进行加权计算得到一步量测预测估计和一步量测估计误差协方差矩阵,对一步状态预测的样本点和一步量测预测的样本点进行加权计算得到状态与量测的互协方差矩阵,具体步骤为:
根据公式(14)分解一步状态预测估计误差协方差矩阵Pk|k-1获得Sk|k-1:
根据公式(15)和(16)计算第一类sigma点X00,k|k-1和该第一类sigma点X00,k|k-1的权值w10:
根据公式(17)和(18)计算具有相同权值的2n个第二类sigma点和该第二类sigma点的权值w11:
根据公式(19)和(20)计算具有相同权值的2n(n-1)个第三类sigma点 和该第三类sigma点的权值w12:
根据公式(21)计算量测方程传播sigma点:
根据公式(22)估计第k步的量测的一步量测预测估计
根据公式(23)估计第k步量测的一步估计误差协方差矩阵Pzz,k|k-1:
根据公式(24)估计状态与量测的互协方差矩阵:
步骤五:状态滤波更新,根据步骤三获得的一步状态预测估计和一步状态预测估计误差协方差矩阵,和步骤四获得的一步量测预测估计、一步量测估计误差协方差矩阵和状态与量测的互协方差矩阵,完成状态的滤波更新,得到状态的线性最小方差估计和估计误差协方差矩阵,完成目标跟踪的状态估计任务,具体过程为:
根据公式(25)估计高阶UKF的滤波增益Wk:
根据公式(26)计算第k步状态滤波估计
根据公式(27)计算第k步状态估计误差协方差矩阵Pk|k:
Priority Applications (1)
Application Number | Priority Date | Filing Date | Title |
---|---|---|---|
CN201310158729.7A CN103278813B (zh) | 2013-05-02 | 2013-05-02 | 一种基于高阶无迹卡尔曼滤波的状态估计方法 |
Applications Claiming Priority (1)
Application Number | Priority Date | Filing Date | Title |
---|---|---|---|
CN201310158729.7A CN103278813B (zh) | 2013-05-02 | 2013-05-02 | 一种基于高阶无迹卡尔曼滤波的状态估计方法 |
Publications (2)
Publication Number | Publication Date |
---|---|
CN103278813A CN103278813A (zh) | 2013-09-04 |
CN103278813B true CN103278813B (zh) | 2015-04-29 |
Family
ID=49061388
Family Applications (1)
Application Number | Title | Priority Date | Filing Date |
---|---|---|---|
CN201310158729.7A Expired - Fee Related CN103278813B (zh) | 2013-05-02 | 2013-05-02 | 一种基于高阶无迹卡尔曼滤波的状态估计方法 |
Country Status (1)
Country | Link |
---|---|
CN (1) | CN103278813B (zh) |
Families Citing this family (17)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
CN103575298A (zh) * | 2013-11-14 | 2014-02-12 | 哈尔滨工程大学 | 基于自调节的ukf失准角初始对准方法 |
CN103792562A (zh) * | 2014-02-24 | 2014-05-14 | 哈尔滨工程大学 | 一种基于变换采样点的强跟踪ukf的滤波方法 |
CN104022757B (zh) * | 2014-06-13 | 2016-10-19 | 中国科学院重庆绿色智能技术研究院 | 一种高阶矩匹配的多层无迹卡尔曼滤波器的线性扩展方法 |
CN104330083A (zh) * | 2014-10-27 | 2015-02-04 | 南京理工大学 | 基于平方根无迹卡尔曼滤波的多机器人协同定位算法 |
CN105137957A (zh) * | 2015-06-23 | 2015-12-09 | 黄红林 | 一种核电蒸汽发生器控制方法 |
CN105703740B (zh) * | 2016-01-13 | 2019-03-22 | 中国科学院重庆绿色智能技术研究院 | 基于多层重要性采样的高斯滤波方法和高斯滤波器 |
CN106526542A (zh) * | 2016-10-17 | 2017-03-22 | 西南大学 | 一种基于确定性采样的增广卡尔曼滤波方法 |
CN107886058B (zh) * | 2017-10-31 | 2021-03-26 | 衢州学院 | 噪声相关的两阶段容积Kalman滤波估计方法及系统 |
CN108255786B (zh) * | 2017-11-28 | 2021-07-16 | 中南大学 | 一种称重结果的干扰补偿计算方法及系统 |
CN109061615A (zh) * | 2018-10-26 | 2018-12-21 | 海鹰企业集团有限责任公司 | 被动声纳中非线性系统的目标运动参数估计方法及装置 |
CN109977849B (zh) * | 2019-03-22 | 2020-03-17 | 东华理工大学 | 一种基于迹变换的图像纹理特征融合提取方法 |
CN110277978A (zh) * | 2019-07-04 | 2019-09-24 | 苏州妙益科技股份有限公司 | 一种基于可变渐消因子的卡尔曼滤波电流采集方法 |
CN110492866B (zh) * | 2019-07-22 | 2022-12-09 | 航天东方红卫星有限公司 | 一种对运动目标的卡尔曼滤波方法 |
CN110501686A (zh) * | 2019-09-19 | 2019-11-26 | 哈尔滨工程大学 | 基于一种新型自适应高阶无迹卡尔曼滤波的状态估计方法 |
CN110912535B (zh) * | 2019-12-11 | 2023-12-15 | 云南大学 | 一种新型无先导卡尔曼滤波方法 |
CN110940999A (zh) * | 2019-12-13 | 2020-03-31 | 合肥工业大学 | 一种基于误差模型下的自适应无迹卡尔曼滤波方法 |
CN112731371B (zh) * | 2020-12-18 | 2024-01-23 | 重庆邮电大学 | 一种激光雷达与视觉融合的集成化目标跟踪系统及方法 |
Citations (1)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
CN101581775A (zh) * | 2009-06-18 | 2009-11-18 | 哈尔滨工业大学 | 基于四维ukf的高动态gnss载波的开环补偿跟踪方法 |
Family Cites Families (1)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
JP5818608B2 (ja) * | 2011-09-27 | 2015-11-18 | インターナショナル・ビジネス・マシーンズ・コーポレーションInternational Business Machines Corporation | カルマン・フィルタの処理方法、プログラム及びシステム |
-
2013
- 2013-05-02 CN CN201310158729.7A patent/CN103278813B/zh not_active Expired - Fee Related
Patent Citations (1)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
CN101581775A (zh) * | 2009-06-18 | 2009-11-18 | 哈尔滨工业大学 | 基于四维ukf的高动态gnss载波的开环补偿跟踪方法 |
Non-Patent Citations (2)
Title |
---|
基于无迹卡尔曼滤波的被动多传感器融合跟踪;杨柏胜 等;《控制与决策》;20080430;第23卷(第4期);第460-463页 * |
基于滤波算法的对比研究;靳璐 等;《火力与指挥控制》;20101031;第35卷(第10期);第127-130页 * |
Also Published As
Publication number | Publication date |
---|---|
CN103278813A (zh) | 2013-09-04 |
Similar Documents
Publication | Publication Date | Title |
---|---|---|
CN103278813B (zh) | 一种基于高阶无迹卡尔曼滤波的状态估计方法 | |
CN106950562B (zh) | 一种基于预测值量测转换的状态融合目标跟踪方法 | |
CN108802721B (zh) | 一种任意直线约束下目标跟踪方法 | |
CN103344260B (zh) | 基于rbckf的捷联惯导系统大方位失准角初始对准方法 | |
CN104252178A (zh) | 一种基于强机动的目标跟踪方法 | |
CN105785358B (zh) | 一种方向余弦坐标系下带多普勒量测的雷达目标跟踪方法 | |
CN108319570B (zh) | 一种异步多传感器空时偏差联合估计与补偿方法及装置 | |
CN104809326A (zh) | 一种异步传感器空间配准算法 | |
CN103902812B (zh) | 一种粒子滤波方法、装置及目标跟踪方法、装置 | |
CN105954742B (zh) | 一种球坐标系下带多普勒观测的雷达目标跟踪方法 | |
UmaMageswari et al. | A comparitive study of Kalman filter, extended kalman filter and unscented Kalman filter for harmonic analysis of the non-stationary signals | |
CN104833949A (zh) | 一种基于改进距离参数化的多无人机协同无源定位方法 | |
CN107290742A (zh) | 一种非线性目标跟踪系统中平方根容积卡尔曼滤波方法 | |
CN103575298A (zh) | 基于自调节的ukf失准角初始对准方法 | |
CN103312297B (zh) | 一种迭代扩展增量卡尔曼滤波方法 | |
CN103616024A (zh) | 一种行星探测进入段自主导航系统可观测度确定方法 | |
CN103296995A (zh) | 任意维高阶(≥4阶)无味变换与无味卡尔曼滤波方法 | |
Goos et al. | Estimation of linear parameter-varying affine state space models using synchronized periodic input and scheduling signals | |
Feng et al. | Robust cubature Kalman filter for SINS/GPS integrated navigation systems with unknown noise statistics | |
CN105549003A (zh) | 一种汽车雷达目标跟踪方法 | |
Hao et al. | Linear minimum mean square error filtering with stochastic linear equality constraints | |
CN104849697A (zh) | 一种基于去偏坐标转换的α-β滤波方法 | |
El-Diasty et al. | A frequency-domain INS/GPS dynamic response method for bridging GPS outages | |
CN104320108A (zh) | 基于ahcif的集中式测量值加权融合方法 | |
Xiong et al. | The linear fitting Kalman filter for nonlinear tracking |
Legal Events
Date | Code | Title | Description |
---|---|---|---|
C06 | Publication | ||
PB01 | Publication | ||
C10 | Entry into substantive examination | ||
SE01 | Entry into force of request for substantive examination | ||
C14 | Grant of patent or utility model | ||
GR01 | Patent grant | ||
CF01 | Termination of patent right due to non-payment of annual fee |
Granted publication date: 20150429 Termination date: 20200502 |
|
CF01 | Termination of patent right due to non-payment of annual fee |