无迹卡尔曼滤波算法
运用无迹卡尔曼滤波的实例
运用无迹卡尔曼滤波的实例无迹卡尔曼滤波是一种常用的估计算法，可以在不知道系统模型的情况下对其进行状态估计。
下面我们来看一个使用无迹卡尔曼滤波的实例。
假设我们有一个小车，我们要通过它的加速度计和陀螺仪来估计小车的速度和位置。
假设小车的加速度计测量值为$a$，陀螺仪测量的角速度为$omega$。
我们可以使用以下状态矢量来描述小车的状态： $$x = begin{bmatrix}pvend{bmatrix}$$其中$p$表示小车的位置，$v$表示小车的速度。
根据物理原理，我们可以得出小车的状态方程：$$begin{aligned}dot{p} &= vdot{v} &= aend{aligned}$$但是，实际上我们并不知道加速度和角速度对小车状态的影响，因此无法确定状态转移矩阵$F$和控制输入矩阵$B$。
这时候我们可以使用无迹卡尔曼滤波来估计小车的状态。
我们首先需要定义观测矢量$z$，它由加速度计和陀螺仪测量值组成：$$z = begin{bmatrix}aomegaend{bmatrix}$$然后，我们可以定义状态转移函数$f$和观测函数$h$：$$begin{aligned}f(x_k, u_k) &= begin{bmatrix}p_k + v_kDelta tv_k + a_kDelta tend{bmatrix}h(x_k) &= begin{bmatrix}v_komega_kend{bmatrix}end{aligned}$$其中$u_k$表示控制输入，$Delta t$表示采样时间。
我们可以使用无迹变换来对$f$和$h$进行非线性变换，从而得到无迹卡尔曼滤波的状态预测和观测预测：$$begin{aligned}hat{x}_{k|k-1} &= f(hat{x}_{k-1|k-1}, u_k)hat{z}_{k} &= h(hat{x}_{k|k-1})end{aligned}$$然后，我们可以计算状态预测的协方差$P_{k|k-1}$和观测预测的协方差$S_k$：$$begin{aligned}P_{k|k-1} &= text{cov}(hat{x}_{k|k-1})S_{k} &= text{cov}(hat{z}_k)end{aligned}$$接下来，我们需要计算卡尔曼增益$K_k$：$$K_k = P_{k|k-1}H_k^T(S_k + R_k)^{-1}$$其中$H_k$表示观测函数的雅可比矩阵，$R_k$表示观测噪声的协方差矩阵。
扩展Kalman滤波(EKF)和无迹卡尔曼滤波(ukf)
三、无迹卡尔曼滤波算法(UKF)
假设n维随机向量x : N(x, Px) ,x通过非线性函数y=f(x) 变换后得到n维的随机变量y。通过UT变换可以以较 高的精度和较低的计算复杂度求得y的均值 y 和方 差 Px 。UT的具体过程可描述如下:
三、无迹卡尔曼滤波算法(UKF)
UKF是用确定的采样来近似状态的后验PDF,可以 有效解决由系统非线性的加剧而引起的滤波发散问 题，但UKF仍是用高斯分布来逼近系统状态的后验概 率密度，所以在系统状态的后验概率密度是非高斯 的情况下，滤波结果将有极大的误差。
三、无迹卡尔曼滤波算法(UKF)
： Matlab程序
dax = 1.5; day = 1.5; % 系统噪声
X = zeros(len,4); X(1,:) = [0, 50, 500, 0]; % 状态模拟的初值
for k=2:len
x = X(k-1,1); vx = X(k-1,2); y = X(k-1,3); vy = X(k-1,4);
F(3,4) = 1;
F(4,4) = 2*ky*vy;
2) = 1;
二、扩展Kalman滤波(EKF)算法
function H = JacobianH(X) % 量测雅可比函数
x = X(1); y = X(3);
H = zeros(2,4);
r = sqrt(x^2+y^2);
二、扩展Kalman滤波(EKF)算法
EKF算法是一种近似方法，它将非线性模型在状态 估计值附近作泰勒级数展开，并在一阶截断，用得 到的一阶近似项作为原状态方程和测量方程近似表 达形式，从而实现线性化同时假定线性化后的状态 依然服从高斯分布，然后对线性化后的系统采用标 准卡尔曼滤波获得状态估计。采用局部线性化技术， 能得到问题局部最优解，但它能否收敛于全局最优 解，取决于函数的非线性强度以及展开点的选择。
扩展Kalman滤波(EKF)和无迹卡尔曼滤波(ukf)
Pkk_1 = Phikk_1*Pk_1*Phikk_1' + Qk;
Pxz = Pkk_1*Hk'; Pzz = Hk*Pxz + Rk;
Pxz*Pzz^-1;
Kk =
Xk = fXk_1 + Kk*Zk_hfX;
Pk = Pkk_1 - Kk*Pzz*Kk';
二、扩展Kalman滤波(EKF)算法
[Xk, Pk, Kk] = ekf(eye(4)+Ft*Ts, Qk, fX, Pk, Hk, Rk, Z(k,:)'-hfX);
X_est(k,:) = Xk';
Hale Waihona Puke end二、扩展Kalman滤波(EKF)算法
figure(1), plot(X_est(:,1),X_est(:,3), '+r')
EKF与UKF
一、背景
普通卡尔曼滤波是在线性高斯情况下利用最小均方误差准则获得 目标的动态估计，适应于过程和测量都属于线性系统， 且误差符 合高斯分布的系统。 但是实际上很多系统都存在一定的非线性， 表现在过程方程 （状态方程）是非线性的，或者观测与状态之间 的关系（测量方程）是非线性的。这种情况下就不能使用一般的卡 尔曼滤波了。解决的方法是将非线性关系进行线性近似，将其转化 成线性问题。 对于非线性问题线性化常用的两大途径： （1） 将非线性环节线性化，对高阶项采用忽略或逼近措施；（EKF） （2）用采样方法近似非线性分布. ( UKF)
三、无迹卡尔曼滤波算法(UKF)
UKF是用确定的采样来近似状态的后验PDF,可以 有效解决由系统非线性的加剧而引起的滤波发散问 题，但UKF仍是用高斯分布来逼近系统状态的后验概 率密度，所以在系统状态的后验概率密度是非高斯 的情况下，滤波结果将有极大的误差。
高阶无迹卡尔曼滤波算法在飞机定位中的应用
高阶无迹卡尔曼滤波算法在飞机定位中的应用刘家学;林松岩【摘要】无迹卡尔曼滤波算法（UKF）在飞机定位和跟踪的过程中精度不够，原因在于误差变量的偏度和峰态在坐标转换过程中对其分布影响很大。
为了解决这一问题，将高阶无迹卡尔曼滤波算法应用到 QAR 数据中。
首先，根据高阶 UT 变换，选取一组样本点（sigma 点）表征 k 时刻最优估计值前四阶矩的分布特征，通过传递得到 k ＋1时刻一步预测值的先验概率分布。
然后以观测数据作为量测值，带入滤波算法得到 k ＋1时刻飞机状态的最优估计值。
最后根据计算机产生的模拟噪声数据和真实的 QAR 数据实现飞机定位的仿真验证。
从仿真结果看，高阶无迹卡尔曼滤波算法比无迹卡尔曼滤波精度更高，误差更小，对 QAR 数据中其他类型的数据形式有一定的借鉴意义。
%The cause of inadequate accuracy of unscented Kalman filter (UKF)in localising and tracing an airplane is due to the very big influence of skewness and kurtosis of error variables on the distribution of coordinate during its transformation process.In order to solve the problem,we applied high-order UKF algorithm to quick access recorder (QAR)data.First,according to high-order unscented transformation (UT)we chose a set of sample points (sigma points)to characterise the distribution feature of the first four moments of optimal estimating valueat time k ,and obtained the priori probability distribution of one-step prediction value at time k +1 through transferring.Then we took the observation data as the measured value,and brought in the filtering algorithm to get the optimal estimation value of airplane state at time k+1.At last,according to computer-generated simulative noise data andactual QAR data we achieved the simulated validation of airplane localisation.From the simulation result it is aware that the high-order UKF algorithm has higher accuracy and less error than UKF,this has certain reference significance to the data form of other types in QAR data.【期刊名称】《计算机应用与软件》【年(卷),期】2016(033)005【总页数】5页(P256-259,264)【关键词】QAR 数据;高阶 UT 变换;高阶无迹卡尔曼滤波【作者】刘家学;林松岩【作者单位】中国民航大学航空自动化学院天津 300300;中国民航大学航空自动化学院天津 300300【正文语种】中文【中图分类】TP391对民航客机进行精确的定位和跟踪尤为重要，但是在以经度、纬度和高度为参考的坐标系中无法有效表示飞机的运动过程，所以常常转化到WGS-84坐标系中进行计算，再通过经度和纬度表示出来。
一种高阶无迹卡尔曼滤波方法
第40卷第5期自动化学报Vol.40,No.5 2014年5月ACTA AUTOMATICA SINICA May,2014一种高阶无迹卡尔曼滤波方法张勇刚1黄玉龙1武哲民1李宁1摘要现有的研究中,高阶无迹变换(Unscented transform,UT)还不存在具体的解析解,因此,无法利用高阶无迹变换获得具备更高精度的高阶无迹卡尔曼滤波器(Unscented Kalmanfilter,UKF).为了解决这一问题,本文在五阶容积变换(Cubature transform,CT)的基础上,通过引入一个自由参数κ,得到高阶无迹变换的解析解,从而获得了高阶无迹卡尔曼滤波器(Unscented Kalmanfilter,UKF).同时验证了现有的五阶容积变换和五阶无迹变换分别是本文所提出的高阶无迹变换在κ=2和κ=6−n时的两个特例.进而分析和讨论了高阶无迹卡尔曼滤波器在系统不同维数条件下κ值的最优选取,并讨论了其稳定性.纯方位跟踪模型和弹道目标再入模型仿真验证了本文方法的正确性,且与现有方法相比具有更高的精度.关键词高阶无迹变换,五阶容积变换,五阶无迹变换,高阶无迹卡尔曼滤波器引用格式张勇刚,黄玉龙,武哲民,李宁.一种高阶无迹卡尔曼滤波方法.自动化学报,2014,40(5):838−848DOI10.3724/SP.J.1004.2014.00838A High Order Unscented Kalman Filtering MethodZHANG Yong-Gang1HUANG Yu-Long1WU Zhe-Min1LI Ning1Abstract Currently there is still no specific analytical solution for the high order unscented transform(UT),thus high order UT can not be used to obtain high order unscented Kalmanfilter(UKF)with higher accuracy.In order to solve this problem,an analytical solution of high order UT is obtained by introducing a free parameterκon the basis offifth-order cubature transform(CT),and the high order UKF is then obtained.It is illustrated that the existingfifth-order CT and fifth-order UT are two special cases of the high order UT whenκ=2andκ=6−n,respectively.Furthermore,the optimal choice of parameterκin the high order UKF is analyzed and discussed for different dimensional systems,and the stability of the proposed method is discussed.Simulations based on the bearings-only tracking model and ballistic object reentry model show that the proposed method is correct and it has better performance as compared with the existing methods. Key words High order unscented transform(UT),fifth-order cubature transform(CT),fifth-order unscented transform, high order unscented Kalmanfilter(UKF)Citation Zhang Yong-Gang,Huang Yu-Long,Wu Zhe-Min,Li Ning.A high order unscented Kalmanfiltering method. Acta Automatica Sinica,2014,40(5):838−848无迹卡尔曼滤波器(Unscented Kalmanfilter, UKF)是一种采用无迹变换(Unscented transform, UT)来计算非线性变换均值和协方差的高斯滤波器[1−2].按照近似概率分布比近似任意的非线性变换容易得多的原理,UT变换:1)通过确定性地选择一组样本点来表征一个概率分布的某些特征,比如均值、协方差等;2)经非线性变换传播这组样本点;3)计算传播后的样本点的均值和协方收稿日期2013-06-05录用日期2013-08-28Manuscript received June5,2013;accepted August28,2013国家自然科学基金(61001154,61201409,61371173),中国博士后科学基金(2013M530147),黑龙江省博士后基金(LBH-Z13052),哈尔滨工程大学中央高校基本科研业务费专项基金(HEUCFX41307)资助Supported by National Natural Science Foundation of China (61001154,61201409,61371173),China Postdoctoral Sci-ence Foundation(2013M530147),Heilongjiang Postdoctoral Fund(LBH-Z13052),and Fundamental Research Funds for the Central Universities of Harbin Engineering University (HEUCFX41307)本文责任编委高会军Recommended by Associate Editor GAO Hui-Jun1.哈尔滨工程大学自动化学院哈尔滨1500011.College of Automation,Harbin Engineering University, Harbin150001差[2−3].UKF的估计精度取决于UT变换计算均值和协方差的精度[2−3].相比于扩展卡尔曼滤波器(Extended Kalmanfilter,EKF),UKF在与其相同的计算量下能提供更好的性能,并且不需要计算雅可比矩阵[3].Lefebvre将UT变换诠释为随机线性回归,从而揭示了UKF优于EKF的原因[1,4].但是,当非线性模型或噪声统计特性不准时,此时,文献[2−3]中的UKF滤波性能变差.为了解决这一问题,人们提出了自适应UKF,它能实时地对模型误差和噪声统计特性进行估计与修正,从而提高UKF 的估计精度[5−7].当UKF用于高维非线性系统时,可能会遇到数值不稳定情况[1,8].Arasaratnam等利用球径容积准则,提出了一种对高维状态估计数值稳定的容积卡尔曼滤波器(Cubature Kalman filter,CKF)[8].事实上CKF是UKF在自由参数κ等于0时的一种特例,为高维状态估计中κ应该取0提供了严格的理论依据[8].Jia等将三阶容积变换(Cubature transform,CT)进行推广,得到了具有任意阶精度的高阶CKF[9].除此之外,UKF受非本5期张勇刚等:一种高阶无迹卡尔曼滤波方法839地采样的影响很大,特别是当系统的非线性为三角函数或指数函数时,滤波可能会发散[10−11].为了解决这个问题,Julier等提出了比例UKF[10],Chang等提出了变换的UKF[11].现有的UKF算法本质上是一种基于二阶UT变换的非线性滤波方法,仅能够匹配非线性函数的二阶泰勒展开项,因此,其精度有限.为了提高它的精度,Julier等在文献[12]中讨论了通过选择一组能精确匹配随机向量前四阶矩的样本点的高阶UT变换方法,这些点的取值及其权值满足一个存在未知变量交叉耦合高阶项的方程组,但该方程组无法解析解出,因此,无法完成高阶UT变换的Sigma点及其权值的选取,从而也无法构成高阶UKF.此外,Lerner从数值积分角度基于单项式精确准则给出了五阶UT变换[13].文献[1]指出,五阶UT变换在高阶UT变换中,通过加入所有非中心Sigma点到原点的距离都相等这一约束,来求解高阶UT变换中的Sigma点及其权值,并不是实质意义上的高阶UT变换.由于五阶UT变换强加了这种约束,使得它的精度低于高阶UT变换.本文的目的即为解决上述问题,提出一种精度更高的高阶UKF方法.在三阶CT变换是二阶UT变换的一种特例这一事实的启发下,首先,验证了文献[9]中的五阶CT变换是文献[12]中的高阶UT变换的一种特例.其次,在五阶CT变换的基础上,通过引入自由参数κ,消除高阶UT变换求解中的未知自由度,进而求解了高阶UT变换中存在未知变量交叉耦合高阶项的方程组,得到了高阶UT变换的解析解,并获得了高阶UKF.同时,本文的分析表明,五阶CT变换和文献[13]中的五阶UT变换分别是本文给出的高阶UT变换在κ=2和κ=6−n时的两个特例,因此,说明本文给出的高阶UKF是一种更广义的高阶滤波器,它能从理论上很好地将现有的五阶CKF和五阶UKF统一到高阶UKF框架下.最后,本文给出了高阶UKF中自由参数κ的最优选取策略,为设计性能最优的高阶UKF提供了理论依据,并讨论了高阶UKF在此最优策略下的稳定性,探讨了其实际应用的可行性.本文的理论分析表明,在高斯假设下,对于二维和三维系统,依据本文给出的自由参数κ的最优选取策略设计的高阶UKF可以获得比五阶CKF和五阶UKF更高的精度.纯方位跟踪模型和弹道目标再入模型的仿真结果验证了本文方法的正确性和优越性.1非线性高斯滤波器考虑如下状态空间形式的离散非线性系统x k=f(x k−1)+n k−1 z k=h(x k)+v k (1)其中,x k∈R n为系统的状态向量,z k∈R m为系统的量测向量,f(·)和h(·)为已知的任意函数,随机系统噪声n k−1是零均值方差为Qk−1的高斯白噪声,随机观测噪声v k是零均值方差为R k的高斯白噪声,n k−1与v k不相关.非线性滤波是根据当前时刻及此前的含噪声的量测来获得系统状态的最小方差估计E[x k|Z k],其中,Z k={z j,1≤j≤k}.如果高斯分布能够很好地近似状态的概率密度,可以使用Kalman滤波器结构(线性最小方差更新准则)的高斯滤波器完成状态估计任务.由于均值和方差能够完全表征高斯分布,高斯滤波器一般结构如下[1,14]:ˆx k|k=ˆx k|k−1+W k(z k−ˆz k|k−1)P k|k=P k|k−1−W k P zz zz,k|k−1W TkW k=P x z,k|k−1P−1zzzz,k|k−1(2)其中ˆx k|k−1=E[f(x k−1)|Z k−1]P k|k−1=E[(x k−ˆx k|k−1)(x k−ˆx k|k−1)T|Z k−1]ˆz k|k−1=E[h(x k)|Z k−1]P z z,k|k−1=E[(z k−ˆz k|k−1)(z k−ˆz k|k−1)T|Z k−1]P x z,k|k−1=E[(x k−ˆx k|k−1)(z k−ˆz k|k−1)T|Z k−1](3)从上述高斯滤波器的一般形式中可以知道,高斯滤波器需要计算两个均值和三个协方差,按照线性最小方差准则完成量测更新,同时高斯滤波器的估计精度取决于式(3)中的均值和协方差的计算精度.采用不同的方法来计算式(3)中的均值和协方差,将得到不同类型的高斯滤波器.例如,采用UT变换方法,得到的是UKF[2−3];采用高斯埃尔米特求积方法,得到的是高斯埃尔米特求积滤波器(Gauss-Hermite quadraturefilter,GHQF)[14];采用容积方法,得到的是CKF[8,9];采用多项式插值方法,得到的是中心差分滤波器(Central differencefilter,CDF)[14]等.本文将重点研究高斯滤波器中的UKF,通过求取高阶UT变换的解析解,进而获得高阶UKF,从而提高UKF的精度.下面将介绍现有高阶UT变换存在的问题.2高阶UT变换问题阐述按照近似概率分布比近似任意的非线性变换容易得多这一原理,UT变换首先通过确定性地选择一组Sigma点来表征一个概率分布的某些特征,例如均值、协方差等;其次,经非线性变换(系统函数或量测函数)传播这组Sigma点,得到变换的样本点;最后,通过计算变换的样本点的均值和协方差,完成式(3)中均值和协方差的计算.而式(3)中的均值和840自动化学报40卷协方差的计算精度又取决于Sigma点选取策略,选取的Sigma点越能表征状态的概率分布(即匹配先验状态的各阶矩统计量越多),式(3)中的均值和协方差的计算精度越高[12].不难发现,式(3)中的数学期望是相对于任意的高斯概率密度.从理论上讲,一个非线性函数对一个任意高斯分布的期望总是可以被转换成另外一个非线性函数对标准高斯分布的期望[1,8−9,11].基于上述原理,本文不失一般性地假设状态的概率分布是标准高斯分布.下面将首先介绍传统的二阶UT变换方法,并说明现有的高阶UT变换存在的问题.2.1二阶UT变换式(3)中的均值和协方差计算问题可以归结为标准高斯随机向量的任意非线性变换的均值和协方差计算问题.x为n维的标准高斯随机向量,y为另一随机向量,y与x的关系为y=f(x),UT变换可用来计算随机向量y的均值和协方差.二阶UT变换对称地选取2n+1个Sigma点,这组Sigma点能够匹配n维高斯随机向量x的均值和协方差及所有的高阶奇次阶矩(Sigma点的对称性)[12].图1给出了二维情况下,二阶UT变换在第一象限选择的Sigma点.将图1中的二维推广到n维,可以知道,二阶UT变换需要两类Sigma点,第一类Sigma点位于坐标原点,点数为1,权值为w0;第二类Sigma点对称地分布在与原点距离为s1的n条坐标轴上,点数为2n,权值均为w[2−3,12]1.对于标准高斯随机向量x∼N(00,I),二阶UT变换的Sigma点及其权值为[12]χ=0,w0=κn+κχi=(n+κ)e i,w i=12(n+κ)χi+n=−(n+κ)e i,w i+n=12(n+κ)(4)其中,e i=[0,···,0,1,0,···,0]T,i=1,2,···,n,即第i个元素为1的单位列向量.对于更一般的高斯随机向量x∼N(¯x,P x),二阶UT变换的Sigma点及其权值为χ=¯x,w0=κ(n+κ)χi=¯x+(n+κ)P x e i,w i=12(n+κ)χi+n=¯x−(n+κ)P x e i,w i+n=12(n+κ)(5)其中,i=1,2,···,n.当κ=3−n时,二阶UT变换能匹配高斯随机向量x的四阶主矩[2−3,12].类似二阶UT变换,我们可以推导高阶UT变换Sigma点和权值需要满足的条件.图1二维情况下二阶UT变换在第一象限选择的Sigma点Fig.1Sigma points chosen by second-order UT for a two dimensional case in thefirst quadrant2.2高阶UT变换当一个随机向量的偏度(三阶矩)和峰态(四阶矩)对它的概率分布的影响很大时,此时,二阶UT变换计算方程(3)获得的协方差可能欠估计[12]. Julier等将二阶UT变换进行推广,得到了高阶UT 变换[12].高阶UT变换对称地选取2n2+1个Sigma点,这组Sigma点能够匹配n维高斯随机向量x的前四阶矩(均值、协方差、偏度、峰态)及所有高阶奇次阶矩.图2给出了二维情况下,高阶UT变换在第一象限选择的Sigma点.将图2中的二维推广到n维可以知道,高阶UT变换需要三类Sigma点,第一类Sigma点位于坐标原点,点数为1,权值为w0;第二类Sigma点对称地分布在与原点距离为s1的n条坐标轴上,点数为2n,权值均为w1;第三类Sigma 点对称地位于(0,···,±s2,···,±s2,···,0)T(第i 个和第j个元素不为0,i<j,i,j=1,2,···,n),点数为4C2n,权值为w2.为了精确地匹配标准高斯随机向量x的均值、协方差、偏度和峰态,高阶UT变换的Sigma点及其权值必须满足以下条件[12]:w0+2n w1+2n(n−1)w2−12w1s21+4(n−1)w2s22−12w1s41+4(n−1)w2s42−34w2s42−3=0(6)要使高阶UT变换的Sigma点能匹配高斯随机向量x的六阶主矩,则高阶UT变换的Sigma点及5期张勇刚等:一种高阶无迹卡尔曼滤波方法841其权值还必须满足以下条件[12]:2w1s61+4(n−1)w2s62−15=0(7)图2二维情况下高阶UT变换在第一象限选择的Sigma点Fig.2Sigma points chosen by high order UT for a two dimensional case in thefirst quadrant虽然从理论上讲,高阶UT变换能够匹配高斯随机向量x的全部四阶矩和六阶主矩,从而获得比二阶UT变换更高的精度,但是上述Sigma点及其权值无法解析求出,导致其实际应用受限.下面本文将依据五阶CT变换的理论基础,通过引入自由参数κ,得到高阶UT变换的解析解,进而获得高阶UKF滤波方法.3一种高阶UKF方法及其参数选择3.1高阶UT变换的Sigma点及其权值解析解对于标准高斯随机向量x∼N(00,I),五阶CT 变换的Sigma点及其权值为[9]χ0=0,w0=2n+2(8)χi1=(n+2)e i1χi1+n=−(n+2)e i1,w1=4−n2(n+2)2i1=1,2,···,n(9)χi2=(n+2)s+i2χi2+0.5n(n−1)=−(n+2)s+i2χi2+n(n−1)=(n+2)s−i2χi2+1.5n(n−1)=−(n+2)s−i2w2=1(n+2)2,i2=1,2,···,0.5n(n−1)(10)其中{s+i2}:={12(e k+e l):k<l,k,l=1,2,···,n}{s−i2}:={12(e k−e l):k<l,k,l=1,2,···,n}(11)通过式(8)∼(11)可知,五阶CT变换需要三类Sigma点,第一类Sigma点位于坐标原点,点数为1,权值为2/(n+2);第二类Sigma点对称地分布在距离原点(n+2)的n条坐标轴上,点数为2n,权值为(4−n)/(2(n+2)2);第三类Sigma点对称地分布于(0,···,±(n+2)/2,···,±(n+2)/2,···,0)T(第k个和第l个元素不为0,k<l,k,l=1,2,···,n),点数为2n(n−1),权值为1/(n+2)2.从而可知,五阶CT变换Sigma点的点数和分布情况与高阶UT变换Sigma点的点数和分布情况十分相似.类比高阶UT变换思想,五阶CT变换的Sigma点及其权值参数如下:w0=2n+2,w1=(4−n)2(n+2)2s1=√n+2,w2=1(n+2)2s2=n+22(12)经过简单的代数运算,很容易验证式(12)给出的五阶CT变换的Sigma点及其权值参数满足方程组(6),从而可知五阶CT变换是高阶UT变换的一种特例.下面将在五阶CT变换的基础上,通过引入一个自由参数κ,得到高阶UT变换的解析解.类比于二阶UT变换[12],令w2=1/(n+κ)2,将其代入方程组(6)中,并求解方程组(6),获得w1,w2,s1,s2的值,当n=4时,有:w0=−2n2+(4−2n)κ2+(4κ+4)n(n+κ)2(4−n)w1=(κ+2−n)22(n+κ)2(4−n),w2=1(n+κ)2s1=(4−n)(n+κ)(κ+2−n),s2=n+κ2(13)当n=4时,此时,只有κ=2方程组(6)才有解,从而有w0=2n+2,w1=(4−n)2(n+2)2s1=√n+2,w2=1(n+2)2s2=n+22(14)842自动化学报40卷将上述结果进行推广,得到一般高斯随机向量x∼N(¯x,P x)的高阶UT变换的Sigma点及其权值的解析解(现在只考虑n=4情况,而n=4情况类似):第一类Sigma点及其权值:χ0=¯x,w0=−2n2+(4−2n)κ2+(4κ+4)n(n+κ)2(4−n)(15)第二类Sigma点及其权值:χi1=¯x+(4−n)(n+κ)(κ+2−n)P x e i1χi1+n=¯x−(4−n)(n+κ)(κ+2−n)P x e i1,w1=(κ+2−n)22(n+κ)2(4−n)i1=1,2,···,n(16)第三类Sigma点及其权值:χi2=¯x+(n+κ)P x s+i2χi2+0.5n(n−1)=¯x−(n+κ)P x s+i2χi2+n(n−1)=¯x+(n+κ)P x s−i2χi2+1.5n(n−1)=¯x−(n+κ)P x s−i2w2=1(n+κ)2(17)其中,i2=1,2,···,0.5n(n−1).从上述的推导过程中不难发现,式(15)∼(17)与式(9)∼(11)相比,当κ=2时,高阶UT变换就是五阶CT变换,当κ=6−n时,不难验证式(13)中s1=s2,此时,高阶UT变换就是五阶UT变换.从以上论述不难得出,文献[9]中提出的五阶CT变换和文献[13]中提出的五阶UT变换是本文高阶UT变换在自由参数κ=2和κ=6−n时的两个特例.值得注意的是,对于四维系统(n=4情况),高阶UT变换只有κ=2时,方程组(6)才有解,且五阶UT变换和五阶CT变换都是高阶UT变换在κ=2时的一种特例,因此,对于四维系统,高阶UT 变换、五阶CT变换和五阶UT变换三者完全等价.由五阶UT变换和五阶CT变换分别是高阶UT变换在不同κ值下的特例这一事实可以知道,κ能调节高阶UT变换的性能.因而通过选取合适的κ值就可以获得性能最优的高阶UT变换,进而获得更高精度的高阶UKF方法.我们将在下一小节探讨这个问题.3.2自由参数κ的最优选取高阶UT变换可以通过选取合适的κ值,使得Sigma点及其分布参数满足式(7),从而能捕获到高斯随机向量x的六阶主距,提高UT变换精度.将式(13)中的w1,w2,s1,s2的值代入式(7)中,经过简单的代数运算,得到如下关于κ的代数方程:(n−1)κ2+(2n2−14n)κ+n3−13n2+60n−60=0(18)当n=1时,式(18)的解为κ=−1,将κ=−1再代入式(13)中,可知此时方程组(6)无解,因此,在一维情况下不存在合适的κ值使得Sigma点能捕获到高斯随机向量x的六阶主距,即从UT变换的精度的角度来看,不存在最优的κ值.当n=1时,根据一元二次方程的有解判据∆=−96n2+480n−240,要想式(18)有解,必须有∆≥0,从而有2≤n≤4(n∈N).而又由于当n=4时,只有κ=2时,方程组(6)才有解,因此只有当n=2或n=3时,κ才存在最优值.当n=2时,求解式(18),得到两个最优κ值,即κ=0.835或κ=19.165.由于CT变换具有好的数值稳定性[8−9],所以最优κ值应选择接近κ=2的值.兼顾数值稳定性,在二维情况下一般取最优值为κ=0.835.同理当n=3时,求解式(18),得到两个最优κ值,即κ=1.417或κ=10.583.为了保证滤波的数值稳定性,在三维情况下一般取最优值为κ=1.417.当n≥5时,此时不存在最优的κ值使得Sigma点能捕获到高斯随机向量x的六阶主距,但是从UT变换的数值稳定角度考虑,可以设定κ=2,即采用五阶CT变换.综上所述,对于二维和三维系统,κ存在最优值,而且当κ取最优值时,高阶UT变换的精度高于五阶CT变换和五阶UT变换的精度;对于四维系统,κ只能取2,此时,高阶UT变换与五阶CT变换、五阶UT变换等价;对于一维和四维以上的系统,从精度角度不存在最优的κ值,但是从数值稳定的角度,可以设定κ=2.3.3一种基于高阶UT变换的高阶UKF方法基于高斯滤波器的一般结构(式(2)和(3)),类似于二阶UKF方法,采用上述的高阶UT变换方法来计算式(3)中的两个均值和三个协方差,将得到高阶UKF.具体的高阶UKF步骤如下:1)状态和量测的一步预测:假设k−1时刻状态向量x k−1的后验概率密度函数p(x k−1|Z k−1)=N(ˆx k−1|k−1,P k−1|k−1)通过Cholesky分解k−1时刻状态向量的估计误差协方差矩阵P k−1|k−1,获得矩阵S k−1|k−1:P k−1|k−1=S k−1|k−1S Tk−1|k−1(19)5期张勇刚等:一种高阶无迹卡尔曼滤波方法843计算状态向量x k −1的第一类Sigma 点χ00,k −1|k −1及其权值w 0:χ00,k −1|k −1=ˆx k −1|k −1w 0=−2n 2+(4−2n )κ2+(4κ+4)n(n +κ)2(4−n )(20)计算状态向量x k −1的第二类Sigma 点χ1i 1,k −1|k −1和χ2i 1,k −1|k −1及其权值w 1(i 1=1,2,···,n ):χ1i 1,k −1|k −1= (4−n )(n +κ)(κ+2−n )S k −1|k −1e i 1+ˆx k −1|k −1χ2i 1,k −1|k −1=− (4−n )(n +κ)(κ+2−n )S k −1|k −1e i 1+ˆx k −1|k −1w 1=(κ+2−n )22(n +κ)2(4−n )(21)计算状态向量x k −1的第三类Sigma 点χ3i 2,k −1|k −1,χ4i 2,k −1|k −1,χ5i 2,k −1|k −1和χ6i 2,k −1|k −1及其权值w 2(i 2=1,2,···,n (n −1)/2):χ3i 2,k −1|k −1=(n +κ)S k −1|k −1s +i 2+ˆx k −1|k −1χ4i 2,k −1|k −1=− (n +κ)S k −1|k −1s +i 2+ˆx k −1|k −1χ5i 2,k −1|k −1= (n +κ)S k −1|k −1s −i 2+ˆx k −1|k −1χ6i 2,k −1|k −1=− (n +κ)S k −1|k −1s −i 2+ˆx k −1|k −1w 2=1(n +κ)2(22)其中 {s +i 2}:={12(e k +e l ):k <l ,k ,l =1,2,···,n }{s −i 2}:={12(e k −e l ):k <l ,k ,l =1,2,···,n }(23)通过非线性系统函数f (·)传播状态向量x k −1的Sigma 点,得到变换的样本点:χ∗00,k |k −1=f (χ00,k −1|k −1)χ∗1i 1,k |k −1=f (χ1i 1,k −1|k −1)χ∗2i 1,k |k −1=f (χ2i 1,k −1|k −1)χ∗3i 2,k |k −1=f (χ3i 2,k −1|k −1)χ∗4i 2,k |k −1=f (χ4i 2,k −1|k −1)χ∗5i 2,k |k −1=f (χ5i 2,k −1|k −1)χ∗6i 2,k |k −1=f (χ6i 2,k −1|k −1)(24)计算k 时刻状态的一步预测值ˆx k |k −1:ˆxk |k −1=E[f (x k −1)|Z k −1]=w 0χ∗00,k |k −1+w 1n i 1=1(χ∗1i 1,k |k −1+χ∗2i 1,k |k −1)+w 2n (n −1)/2 i 2=1(χ∗3i 2,k |k −1+χ∗4i 2,k |k −1+χ∗5i 2,k |k −1+χ∗6i 2,k |k −1)(25)计算k 时刻的状态一步预测估计误差协方差P k |k −1:P k |k −1=E[(x k −ˆxk |k −1)(x k −ˆx k |k −1)T |Z k −1]=w 0χ∗00,k |k −1χ∗T00,k |k −1+w 1n i 1=1(χ∗1i 1,k |k −1χ∗T 1i 1,k |k −1+χ∗2i 1,k |k −1χ∗T 2i 1,k |k −1)+w 2n (n −1)/2 i 2=1(χ∗3i 2,k |k −1χ∗T 3i 2,k |k −1+χ∗4i 2,k |k −1χ∗T 4i 2,k |k −1+χ∗5i 2,k |k −1χ∗T 5i 2,k |k −1+χ∗6i 2,k |k −1χ∗T 6i 2,k |k −1)−ˆx k |k −1ˆx T k |k −1+Q k −1(26)假设k 时刻状态向量x k 的先验概率密度函数p (x k |Z k −1)=N(ˆxk |k −1,P k |k −1),通过Cholesky 分解P k |k −1,得到S k |k −1:P k |k −1=S k |k −1S T k |k −1(27)计算状态向量x k 的第一类Sigma 点及其权值:χ00,k |k −1=ˆxk |k −1w 0=−2n 2+(4−2n )κ2+(4κ+4)n(n +κ)2(4−n )(28)计算状态向量x k 的第二类Sigma 点χ1i 1,k |k −1和χ2i 1,k |k −1及其权值w 1(i 1=1,2,···,n ):χ1i 1,k |k −1= (4−n )(n +κ)(κ+2−n )S k |k −1e i 1+ˆx k |k −1χ2i 1,k |k −1=− (4−n )(n +κ)(κ+2−n )S k |k −1e i 1+ˆx k |k −1w 1=(κ+2−n )22(n +κ)2(4−n )(29)计算状态向量x k 的第三类Sigma 点χ3i 2,k |k −1,χ4i 2,k |k −1,χ5i 2,k |k −1和χ6i 2,k |k −1及其权值w 2(i 2=844自动化学报40卷1,2,···,n(n−1)/2):χ3i2,k|k−1=(n+κ)S k|k−1s+i2+ˆx k|k−1χ4i2,k|k−1=−(n+κ)S k|k−1s+i2+ˆx k|k−1χ5i2,k|k−1=(n+κ)S k|k−1s−i2+ˆx k|k−1χ6i2,k|k−1=−(n+κ)S k|k−1s−i2+ˆx k|k−1w2=1(n+κ)2(30)通过非线性量测函数h(·)传播状态向量x k的Sigma点,得到变换的样本点:z00,k|k−1=h(χ00,k|k−1)z1i1,k|k−1=h(χ1i1,k|k−1)z2i1,k|k−1=h(χ2i1,k|k−1)z3i2,k|k−1=h(χ3i2,k|k−1)z4i2,k|k−1=h(χ4i2,k|k−1)z5i2,k|k−1=h(χ5i2,k|k−1)z6i2,k|k−1=h(χ6i2,k|k−1)(31)计算k时刻量测的一步预测值ˆz k|k−1:ˆz k|k−1=E[h(x k)|Z k−1]=w0z00,k|k−1+w1ni1=1(z1i1,k|k−1+z2i1,k|k−1)+w2n(n−1)/2i2=1(z3i2,k|k−1+z4i2,k|k−1+z5i2,k|k−1+z6i2,k|k−1)(32)计算k时刻量测的一步预测估计误差协方差阵P zz z,k|k−1:P zz,k|k−1=E[(z k−ˆz k|k−1)(z k−ˆz k|k−1)T|Z k−1]=w0z00,k|k−1z T00,k|k−1+w1ni1=1(z1i1,k|k−1z T1i1,k|k−1+z2i1,k|k−1z T2i1,k|k−1)+w2n(n−1)/2i2=1(z3i2,k|k−1z T3i2,k|k−1+z4i2,k|k−1z T4i2,k|k−1+z5i2,k|k−1×z T5i2,k|k−1+z6i2,k|k−1z T6i2,k|k−1)−ˆz k|k−1ˆz Tk|k−1+R k(33)计算k时刻状态与量测的互相关协方差矩阵:P x z,k|k−1=E[(x k−ˆx k|k−1)(z k−ˆz k|k−1)T|Z k−1]=w0χ00,k|k−1z T00,k|k−1+w1ni1=1(χ1i1,k|k−1z T1i1,k|k−1+χ2i1,k|k−1z T2i1,k|k−1)+w2n(n−1)/2i2=1(χ3i2,k|k−1z T3i2,k|k−1+χ4i2,k|k−1z T4i2,k|k−1+χ5i2,k|k−1×z T5i2,k|k−1+χ6i2,k|k−1z T6i2,k|k−1)−ˆx k|k−1ˆz Tk|k−1(34)2)滤波更新:计算k时刻高阶UKF的滤波增益W k:W k=P x z,k|k−1P−1z z,k|k−1(35)计算k时刻高阶UKF的状态估计ˆx k|k:ˆx k|k=ˆx k|k−1+W k(z k−ˆz k|k−1)(36)计算k时刻高阶UKF的状态估计误差协方差矩阵P k|k:P k|k=P k|k−1−W k P z z,k|k−1W Tk(37)将第3.2节中自由参数κ的选取策略应用到高阶UKF中,在系统维数为二维或三维条件下,得到的高阶UKF将具有比五阶UKF和五阶CKF更加优越的性能.3.4高阶UKF稳定性讨论高阶UKF稳定性是其实用性的关键指标之一.由于高阶UKF的性能在很大程度上取决于高阶UT变换的性能,所以只有保证高阶UT变换的稳定性,才有可能使高阶UKF稳定.文献[1]提出采用权值的绝对值之和作为衡量积分公式(UT变换也是一种积分公式)数值稳定性的指标,如果|w i|=1,那么数值积分公式是完全稳定的;如果|w i| 1,那么这种积分公式会引入很大的舍入误差,严重时出现数值不稳定.由于高阶UT变换中所有Sigma点的权值之和为1,因此,当所有权值都大于等于零时,即w i≥0(i=0,1,2),此时,|w i|=1恒成立,那么高阶UT变换是完全稳定的.下面将针对不同的系统维数具体分析本算法的稳定性.1)n≤3时,高阶UT变换稳定性.由式(13)和(14)可以知道,当系统维数n≤3时,此时只有w0可能会小于0,并且w0的正负完全取决于自由参数κ的选取.当系统维数n=1,5期张勇刚等:一种高阶无迹卡尔曼滤波方法845n =2和n =3时,由式(13)可以知道此时的权值w 0与J =−2n 2+(4−2n )κ2+(4κ+4)n 同号,当n =1时,J =2(κ+1)2≥0;当n =2时,J =8κ,按照第3.2节κ值的选取策略,此时κ=0.835,从而J >0;当n =3时,J =−2κ2+12κ−6,同样按照第3.2节κ值的选取策略,此时κ=1.417,从而J >0.因此,当n ≤3时,按照第3.2节κ值的选取策略,所有权值满足w i ≥0(i =0,1,2),因此,高阶UT 变换是完全稳定的.2)n =4时,高阶UT 变换稳定性.由式(13)和(14)可以知道,当系统维数n =4时,所有权值满足w i ≥0(i =0,1,2),因此,高阶UT 变换是完全稳定的.3)n ≥5时,高阶UT 变换稳定性.按照第3.2节κ值的选取策略,高阶UT 变换与五阶CT 变换等价.依据文献[9]中的论述,五阶CT 变换在n ≥5时是稳定的,从而此时的高阶UT 变换也是稳定的.综上所述,依据第3.2节给出的κ值最优选取策略,能保证高阶UT 变换在任意维数应用中都是数值稳定的.实际应用中,高阶UT 变换的稳定并不能完全保证高阶UKF 算法的稳定.文献[9]指出当系统包含强非线性或大的不确定性(即大的系统噪声)时,二阶UKF 的滤波性能会大大地下降,严重时甚至会出现滤波发散,同样高阶UKF 也会出现类似的情况,此时可以通过适当地增大系统噪声方差矩阵来提高高阶UKF 滤波稳定性[15].4仿真为了验证本文提出的高阶UKF 方法的正确性和优越性,我们采用与文献[1,14,16−17]相同的纯方位跟踪模型和弹道目标再入模型仿真.4.1纯方位跟踪模型仿真纯方位跟踪模型为二维非线性模型,其离散模型如下[16]:x k =0.9001 x k −1+n k −1(38)z k =tan −1 x 2,k−sin(k )x 1,k −cos(k )+v k(39)其中状态x k =[x 1,k x 2,k ]T =[s t ]T,表示s -t 平面内(笛卡尔坐标系)的位置,系统噪声n k ∼N(00,Q k ),Q k = 0.10.050.050.1,观测噪声v k ∼N(00,R k ),R k =0.025.初始状态真实值x 0=[205]T ,初始协方差阵P 0|0=[0.10;00.1].与文献[16]相同,仿真过程中采用如下定义的均方误差性能指标,比较各种滤波方法的性能:MSE(k )=1N Nn =1(x n i ,k −x n i ,k |k )2,i =1,2(40)其中,N 为Monte Carlo 仿真次数.仿真时间100秒,在相同初始条件下采用二阶UKF (κ=1)、三阶CKF 、本文提出的高阶UKF (κ=0.835)、五阶UKF 、五阶CKF 分别进行250次Monte Carlo 仿真.仿真结果如下:从图3和图4的状态跟踪曲线可以知道,二阶UKF (图中点线)(κ=1)和三阶CKF (图中实线)在24s 后对真实状态(图中星号线)的跟踪性能变差,但二阶UKF (κ=1)的跟踪性能优于三阶CKF.整个过程中五阶CKF (图中加号线)和五阶UKF (图中点划线)都能较好地跟踪上真实的状态,但它们的跟踪性能都不如本文提出的高阶UKF (κ=0.835)(图中虚线).图3位置状态x (1)的估计Fig.3Estimates of position state x (1)图4位置状态x (2)的估计Fig.4Estimates of position state x (2)846自动化学报40卷从图5和图6的状态估计的均方误差曲线可以知道,二阶UKF (图中点线)(κ=1)的精度高于三阶CKF(图中实线),五阶CKF(图中中等粗的实线)和五阶UKF(图中中等粗的点线)的精度都高于二阶UKF(κ=1),但是这些滤波方法中本文提出的高阶UKF(κ=0.835)(图中最粗的实线)精度最高.上述结论用符号表示如下:三阶CKF<二阶UKF<五阶CKF和五阶UKF<高阶UKF(κ=0.835)理论上,在二维情况下,二阶UT变换(κ=1)计算的均值能部分地精确到四阶,三阶CT变换即二阶UT变换(κ=0)的均值计算精度为三阶,五阶UT变换和五阶CT变换的均值计算精度为五阶,而本文的高阶UT变换在κ=0.835时计算的均值能部分地精确到六阶.由于UKF(CKF)的精度取决于UT(CT)变换的精度,因此在二维情况下,本文所提出的高阶UKF(κ=0.835)精度高于五阶CKF和五阶UKF,同时五阶CKF和五阶UKF的精度都高于二阶UKF(κ=1),二阶UKF(κ=1)的精度高于三阶CKF.仿真结果验证了上述理论分析的正确性,也体现了本文提出的高阶UKF算法的优越性.图5位置状态x(1)的均方误差Fig.5Mean square error of position state x(1)图6位置状态x(2)的均方误差Fig.6Mean square error of position state x(2)4.2弹道目标再入模型仿真弹道目标再入模型为三阶非线性系统,其目标连续时间动力学方程为[1,14,17]˙x1(t)=−x2(t)˙x2(t)=−e−γx1(t)x22(t)x3(t)˙x3(t)=0(41)主要目的是对从高处以很快速度进入大气层的载体的位置x1、速度x2和弹道常数x3进行估计.常数γ表示大气密度和高度间的关系.载体的位置通过雷达测定,它们之间的关系为y k=M2+(x1,k−H)2+v k(42)其中,H为雷达高度,M为其与载体之间的水平距离.v k∼N(00,R k).雷达每1秒测量一次间距.为了消除系统强非线性的影响,通常在两次观测之间使用64次四阶Runge-Kutta方法对式(41)进行积分[1,14,17].仿真过程中,所有的滤波器都以小步长(ft)在每个点处进行预测,并在观测量到来之前计算均值和协方差矩阵.系统仿真参数如下:γ=5×10−5,H=105ft,M=105ft,R k=104ft2系统真实状态初值x0=[3×1052×10410−3]T,仿真中滤波初始值设为ˆx0|0=[3×1052×1043×10−5]T,协方差矩阵P0|0=diag{1064×10610−4}.与文献[1,14,17]相同,仿真中采用如下定义的平均绝对值误差比较各种滤波方法:λ(k)=1NNn=1|x nk−ˆx nk|k|(43)其中,N为Monte Carlo次数.仿真时间60秒,在相同初始条件下采用二阶UKF(κ=0)(此时与三阶CKF等价)、高阶UKF (κ=1.417)、五阶UKF、五阶CKF分别进行250次Monte Carlo仿真.仿真结果如下:从图7∼图9可以知道,在三维情况下,五阶CKF(图中点线)和五阶UKF(图中粗实线)的精度几乎一样,而且五阶CKF和五阶UKF的精度高于二阶UKF(κ=0)(图中实线),但是这些滤波方法中本文提出的高阶UKF(κ=1.417)(图中粗点线)精度最高.上述结论用符号表示如下:5期张勇刚等:一种高阶无迹卡尔曼滤波方法847图7位置的平均绝对值误差Fig.7Averaged absolute error of position图8速度的平均绝对值误差Fig.8Averaged absolute error of velocity图9弹道系数的平均绝对值误差Fig.9Averaged absolute error of ballistic coefficient 三阶CKF=二阶UKF<五阶CKF和五阶UKF<高阶UKF(κ=1.417)理论上,在三维情况下,二阶UT变换(κ=0)与三阶CT变换是等价的,它们计算的均值都能部分地精确到四阶,五阶UT变换和五阶CT变换的均值计算精度为五阶,而本文的高阶UT变换在κ=1.417时,计算的均值能部分地精确到六阶.由于UKF(CKF)的精度取决于UT(CT)变换的精度,因此在三维情况下,本文的高阶UKF (κ=1.417)精度高于五阶CKF和五阶UKF,同时五阶CKF和五阶UKF的精度都高于二阶UKF (κ=0)或三阶CKF.仿真结果验证了上述理论分析的正确性,也体现了本文提出的高阶UKF算法的优越性.5结论本文在五阶CT变换的基础上,通过引入一个自由参数κ,得到了高阶UT变换的解析解,进而获得了高阶UKF方法.在高斯假设下,分析并证明了二维和三维系统可以通过选取合适的κ值来匹配随机向量的六阶主矩,从而提高UKF精度.仿真结果表明,对于二维和三维系统分别选取κ=0.835和κ=1.417,可以获得比五阶UKF和五阶CKF更高的滤波精度,而对于一维系统和三维以上的系统从数值稳定性的角度来讲应该选取κ=2.本文的分析为高阶UKF的实际应用提供了理论依据.References1Wu Y X,Hu D W,Wu M P,Hu X P.A numerical-integration perspective on Gaussianfilters.IEEE Transactions on Signal Processing,2006,54(8):2910−29212Julier S J,Uhlman J K,Durrant-Whyte H F.A new method for the nonlinear transformation of means and covariances infilters and estimators.IEEE Transactions on Automatic Control,2000,45(3):477−4823Julier S J,Uhlman J K.Unscentedfiltering and nonlinear estimation.Proceedings of the IEEE,2004,92(3):401−4224Lefebvre T,Bruyninckx H,de Schuller ment on “A new method for the nonlinear transformation of means and covariances infilters and estimators”[and author s re-ply].IEEE Transactions on Automatic Control,2002,47(8): 1406−14095Zhao Lin,Wang Xiao-Xu,Sun Ming,Ding Ji-Cheng,Yan Chao.Adaptive UKFfiltering algorithm based on maximuma posterior estimation and exponential weighting.Acta Au-tomatica Sinica,2010,36(7):1007−1019(赵琳,王小旭,孙明,丁继成,闫超.基于极大后验估计和指数加权的自适应UKF滤波算法.自动化学报,2010,36(7):1007−1019) 6Sun Yao,Zhang Qiang,Wan Lei.Small autonomous un-derwater vehicle navigation system based on adaptive UKF algorithm.Acta Automatica Sinica,2010,37(3):342−353 (孙尧,张强,万磊.基于自适应UKF算法的小型水下机器人导航系统.自动化学报,2010,37(3):342−353)7Wang Lu,Li Guang-Chun,Qiao Xiang-Wei,Wang Zhao-Long,Ma Tao.An adaptive UKF algorithm based on maxi-mum likelihood principle and expectation maximization al-gorithm.Acta Automatica Sinica,2012,38(7):1200−1210 (王璐,李光春,乔相伟,王兆龙,马涛.基于极大似然准则和最大期望算法的自适应UKF算法.自动化学报,2012,38(7): 1200−1210)8Arasaratnam I,Haykin S.Cubature Kalmanfilters.IEEE Transactions on Automatic Control,2009,54(6): 1254−12699Jia B,Xin M,Cheng Y.High-degree cubature Kalmanfilter.Automatica,2013,49(2):510−518。
卡尔曼滤波、扩展卡尔曼滤波、无迹卡尔曼滤波以及粒子滤波原理
卡尔曼滤波、扩展卡尔曼滤波、无迹卡尔曼滤波以及粒子滤波原理所有滤波问题其实都是求感兴趣的状态的后验概率分布，只是由于针对特定条件的不同，可通过求解递推贝叶斯公式获得后验概率的解析解（KF、EKF、UKF），也可通过大数统计平均求期望的方法来获得后验概率（PF）。
1 KF、EKF、UKF1.1 定义KF、EKF、UKF 都是一个隐马尔科夫模型与贝叶斯定理的联合实现。
是通过观测信息及状态转移及观测模型对状态进行光滑、滤波及预测的方法。
而KF、EKF及UKF的滤波问题都可以通过贝叶斯估计状态信息的后验概率分布来求解。
Kalman在线性高斯的假设下，可以直接获得后验概率的解析解；EKF是非线性高斯模型，通过泰勒分解将非线性问题转化为线性问题，然后套用KF的方法求解，缺陷是线性化引入了线性误差且雅克比、海塞矩阵计算量大；而UKF也是非线性高斯模型，通过用有限的参数来近似随机量的统计特性，用统计的方法计算递推贝叶斯中各个积分项，从而获得了后验概率的均值和方差。
1.2 原理KF、EKF、UKF滤波问题是一个隐马尔科夫模型与贝叶斯定理的联合实现。
一般的状态模型可分为状态转移方程和观测方程，而状态一般都是无法直接观测到的，所以时隐马尔科夫模型。
然后，它将上一时刻获得的状态信息的后验分布作为新的先验分布，利用贝叶斯定理，建立一个贝叶斯递推过程，从而得到了贝叶斯递推公式，像常用的卡尔曼滤波、扩展卡尔曼滤波、不敏卡尔曼滤波以及粒子滤波都是通过不同模型假设来近似最优贝叶斯滤波得到的。
这也是滤波问题的基本思路。
所有贝叶斯估计问题的目的都是求解感兴趣参数的后验概率密度。
并且后验概率的求解是通过递推计算目标状态后验概率密度的方法获得的。
在贝叶斯框架下，通过状态参数的先验概率密度和观测似然函数来求解估计问题；在目标跟踪背景下（隐马尔科夫模型），目标动态方差决定状态转移概率，观测方程决定释然函数。
一般化的整个计算过程可以分为3步：01. 一步状态预测：通过状态转移概率及上一时刻的后验概率算出一步预测概率分布。
无迹卡尔曼滤波（UnscentedKalmanFilter）
⽆迹卡尔曼滤波（UnscentedKalmanFilter）
⽆迹卡尔曼滤波不同于扩展卡尔曼滤波，它是概率密度分布的近似，由于没有将⾼阶项忽略，所以在求解⾮线性时精度较⾼。
UT变换的核⼼思想：近似⼀种概率分布⽐近似任意⼀个⾮线性函数或⾮线性变换要容易。
原理：
假设n维随机向量x:N(x均值，Px)，x通过⾮线性函数y=f(x)变换后得到n维的随机变量y。
通过UT变换可以⽐较⾼的精度和较低的计算复杂度求得y的均值和⽅差Px。
UT的具体过程如下：
（1）计算2n+1个Sigma点及其权值：
根号下为矩阵平⽅根的第i列
依次为均值、⽅差的权值
式中：
α决定Sigma点的散步程度，通常取⼀⼩的正值；k通常取0；β⽤来描述x的分布信息，⾼斯情况下，β的最优值为2。
（2）计算Sigma点通过⾮线性函数f()的结果：
从⽽得知
由于x的均值和⽅差都精确到⼆阶，计算得到y的均值和⽅差也精确到⼆阶，⽐线性化模型精度更⾼。
EKF、UKF、PF组合导航算法仿真对比分析
EKF、UKF、PF组合导航算法仿真对比分析摘要随着人类对海洋探索的逐步深入，自主式水下机器人已被广泛地应用于海底搜救、石油开采、军事研究等领域。
良好的导航性能可以为航行过程提供准确的定位、速度和姿态信息，有利于AUV精准作业和安全回收。
本文介绍了三种不同的导航算法的基本原理，并对算法性能进行了仿真实验分析。
结果表明，在系统模型和时间步长相同的情况下，粒子滤波算法性能优于无迹卡尔曼滤波算法，无迹卡尔曼滤波算法性能优于扩展卡尔曼滤波算法。
关键词自主式水下机器人导航粒子滤波无迹卡尔曼滤波扩展卡尔曼滤波海洋蕴藏着丰富的矿产资源、生物资源和其他能源，但海洋能见度低、环境复杂、未知度高，使人类探索海洋充满了挑战。
自主式水下机器人（Autonomous Underwater Vehicle，AUV)可以代替人类进行海底勘探、取样等任务[1]，是人类探索和开发海洋的重要工具，已被广泛地应用于海底搜救、石油开采、军事研究等领域。
为了使其具有较好的导航性能，准确到达目的地，通常采用组合导航算法为其导航定位。
常用的几种组合导航算法有扩展卡尔曼滤波算法（Extended Kalman Filter，EKF）、无迹卡尔曼滤波算法（Unscented Kalman Filter，UKF）和粒子滤波算法（Particle Filter，PF）。
1扩展卡尔曼滤波算法EKF滤波算法通过泰勒公式对非线性系统的测量方程和状态方程进行一阶线性化截断，主要包括预测阶段和更新阶段。
预测阶段是利用上一时刻的状态变量和协方差矩阵来预测当前时刻的状态变量和协方差矩阵；更新阶段是通过计算卡尔曼增益，并结合预测阶段更新的状态变量和当前时刻的测量值，进而更新状态变量和协方差矩阵[2]。
虽然EKF滤波算法在非线性状态估计系统中广泛应用，但也凸显出两个问题：一是由于泰勒展开式抛弃了高阶项导致截断误差产生，所以当系统处于强非线性、非高斯环境时，EKF算法可能会使滤波发散；二是由于EKF算法在线性化处理时需要用雅克比(Jacobian)矩阵，其繁琐的计算过程导致该方法实现相对困难。
- 1、下载文档前请自行甄别文档内容的完整性，平台不提供额外的编辑、内容补充、找答案等附加服务。
- 2、"仅部分预览"的文档,不可在线预览部分如存在完整性等问题,可反馈申请退款(可完整预览的文档不适用该条件!)。
- 3、如文档侵犯您的权益，请联系客服反馈,我们会尽快为您处理(人工客服工作时间：9:00-18:30)。
%该文件用于编写无迹卡尔曼滤波算法及其测试%注解：主要子程序包括：轨迹发生器、系统方程% 测量方程、UKF滤波器%作者：Jiangfeng%日期：2012.4.16%---------------------------------------function UKFmain%------------------清屏----------------close all;clear all;clc; tic;global Qf n; %定义全局变量%------------------初始化--------------stater0=[220; 1;55;-0.5]; %标准系统初值state0=[200;1.3;50;-0.3]; %测量状态初值%--------系统滤波初始化p=[0.005 0 0 0;0 0.005 0 0;0 0 0.005 0;0 0 0 0.005]; %状态误差协方差初值n=4; T=3;Qf=[T^2/2 0;0 T;T^2/2 0;0 T];%--------------------------------------stater=stater0;state=state0; xc=state;staterout=[]; stateout=[];xcout=[];errorout=[];tout=[];t0=1; h=1; tf=1000; %仿真时间设置%---------------滤波算法----------------for t=t0:h:tf[state,stater,yc]=track(state,stater); %轨迹发生器：标准轨迹和输出[xc,p]=UKFfiter(@systemfun,@measurefun,xc,yc,p);error=xc-stater; %滤波处理后的误差staterout=[staterout,stater];stateout=[stateout,state];errorout=[errorout,error];xcout=[xcout,xc];tout=[tout,t];end%---------------状态信息图像---------------figure;plot(tout,xcout(1,:),'r',tout,staterout(1,:),'g',...tout,stateout(1,:),'black');legend('滤波后','真实值','无滤波');grid on;xlabel('时间 t（s）');ylabel('系统状态A');title('无迹卡尔曼滤波');figure;plot(tout,xcout(2,:),'r',tout,staterout(2,:),'g',...tout,stateout(2,:),'black');grid on;legend('滤波后','真实值','无滤波');xlabel('时间 t（s）');ylabel('系统状态B');title('无迹卡尔曼滤波');figure;plot(tout,xcout(3,:),'r',tout,staterout(3,:),'g',...tout,stateout(3,:),'black');grid on;legend('滤波后','真实值','无滤波');xlabel('时间 t（s）');ylabel('系统状态C');title('无迹卡尔曼滤波');figure;plot(tout,xcout(4,:),'r',tout,staterout(4,:),'g',...tout,stateout(4,:),'black');grid on;legend('滤波后','真实值','无滤波');xlabel('时间 t（s）');ylabel('系统状态D');title('无迹卡尔曼滤波');figure;plot(tout,errorout(1,:),'r',tout,errorout(2,:),'g',...tout,errorout(3,:),'black',tout,errorout(4,:),'b'); grid on;legend('A','B','C','D');xlabel('时间 t（s）');ylabel('滤波后的状态误差');title('无迹卡尔曼滤波误差');%---------------------------------------------toc; %计算仿真程序运行时间endfunction [state,stater,yout]=track(state0,stater0)%-----------------------------------%轨迹发生函数%-----------------------------------T=3;F=[1 T 0 0;0 1 0 0;0 0 1 T;0 0 0 1];G=[T^2/2 0;0 T;T^2/2 0;0 T];V=0.005*randn(2,1);W=0.008*randn(1,1);state=F*state0+G*V;stater=F*stater0;yout=atan(stater0(3)/stater0(1))+W;%用真实值得到测量值，在滤波时结果才会与真实值重合。
endfunction state=systemfun(state0)%-------------------------%系统方程%-------------------------T=3;F=[1 T 0 0;0 1 0 0;0 0 1 T;0 0 0 1];state=F*state0;endfunction yout=measurefun(state0)%----------------------------%测量方程%----------------------------yout=atan(state0(3)/state0(1));endfunction [xc,p]=UKFfiter(systemfun,measurefun,xc0,yc,p0)%------------------------------------------%此程序用于描述UKF（无迹kalman滤波）算法%作者: Jiangfeng%日期：2012.3.17%------------------------------------------global Qf n;%----------------参数注解-------------------% xc0---状态初值(列向量) yc---系统测量值% p0----状态误差协方差 n----系统状态量数%systemfun---系统方程 measurefun--测量方程%---------------滤波初始化------------------alp=1; %default, tunable kap=-1; %default, tunablebeta=2; %default, tunablelamda=alp^2*(n+kap)-n; %scaling factornc=n+lamda; %scaling factor Wm=[lamda/nc 0.5/nc+zeros(1,2*n)]; %weights for meansWc=Wm;Wc(1)=Wc(1)+(1-alp^2+beta); %weights for covariance ns=sqrt(nc);%-------------------------------------------sxk=0;spk=0;syk=0;pyy=0;pxy=0; p=p0;%--------------构造sigma点-----------------pk=ns*chol(p); % B=chol(A);meant:A'*A=B;sigma=xc0;for k=1:2*nif(k<=n)sigma=[sigma,xc0+pk(:,k)];elsesigma=[sigma,xc0-pk(:,k-n)];endend%-------------时间传播方程----------------for ks=1:2*n+1sigma(:,ks)=systemfun(sigma(:,ks));%利用系统方程对状态预测sxk=Wm(ks)*sigma(:,ks)+sxk;end%----------完成对Pk的估计for kp=1:2*n+1spk=Wc(kp)*(sigma(:,kp)-sxk)*(sigma(:,kp)-sxk)'+spk;endspk=spk+Qf*0.005*Qf';%-----------------------for kg=1:2*n+1gamma(kg)=measurefun(sigma(:,kg));endfor ky=1:2*n+1syk=syk+Wm(ky)*gamma(ky);end%--------------测量更新方程--------------for kpy=1:2*n+1pyy=Wc(kpy)*(gamma(kpy)-syk)*(gamma(kpy)-syk)'+pyy;endpyy=pyy+0.008;for kxy=1:2*n+1pxy=Wc(kxy)*(sigma(:,kxy)-sxk)*(gamma(kxy)-syk)'+pxy;endkgs=pxy/pyy; %修正系数xc=sxk+kgs*(yc-syk); %测量信息修正状态p=spk-kgs*pyy*kgs'; %误差协方差阵更新%-------------------------------------end。