无迹卡尔曼滤波算法
运用无迹卡尔曼滤波的实例
运用无迹卡尔曼滤波的实例无迹卡尔曼滤波是一种常用的估计算法，可以在不知道系统模型的情况下对其进行状态估计。
下面我们来看一个使用无迹卡尔曼滤波的实例。
假设我们有一个小车，我们要通过它的加速度计和陀螺仪来估计小车的速度和位置。
假设小车的加速度计测量值为$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)矩阵，其繁琐的计算过程导致该方法实现相对困难。
自适应平方根球型无迹卡尔曼滤波算法
自适应平方根球型无迹卡尔曼滤波算法
自适应平方根球型无迹卡尔曼滤波算法（Adaptive Square-Root Cubature Kalman Filtering Algorithm）在近年来受到了互联网行业的广泛关注。
这种算法归结为：一种利用滤波理论从一系列混合系统动态中估计和提取信息的空间序列分析技术。
它有效解决了复杂系统动态状态参数估计问题，可以有效应用在大数据环境中，进而提升了信息提取和处理的准确性以及效率。
算法的优势体现在数据处理的准确性、可靠性与时效性的发挥上。
由于算法的
自适应特性，估计目标状态参数的准确性得到了很大提升，可以更有效的发挥数据的价值，从而更好的满足用户的需求。
此外，无迹的特性也使得整体数据处理过程具备更强的鲁棒性，对于复杂系统可以获得更为稳定的参数估计，能够很好地抵抗外部噪声干扰。
此外，算法的弹性也得到了极大加强，它可以很好地应对复杂系统动态的变化，而且具有更强的自适应性，能够有效的去除虚假的数据，并根据新的条件快速调节状态参数，从而大幅提高数据处理的速度与准确度。
通过上述分析，自适应平方根球型无迹卡尔曼滤波算法在互联网行业中有非常
重要的应用价值。
在大数据环境下，借助该算法，能够更加快速且准确地处理复杂系统动态状态参数变化，从而有效地促进了信息提取与处理的发展，有助于提高用户体验，实现互联网行业的健康发展。
高斯和高阶无迹卡尔曼滤波算法
高斯和高阶无迹卡尔曼滤波算法王磊;程向红;李双喜【摘要】为了提高非线性变换的近似精度,提出了一种高阶无迹变换(High order Unscented Transform,HUT)机制,利用HUT确定采样点并进行数值积分去近似状态的后验概率密度函数,建立了高阶无迹卡尔曼滤波(High-order Unscented Kalman Filter,HUKF)算法.进一步的为了解决非线性、非高斯系统的状态估计问题,将HUKF与高斯和滤波(Gaussian Sum Filter,GSF)相结合,提出了一种高斯和高阶无迹卡尔曼滤波算法(Gaussian Sum High order Unscented Kalman filter,GS-HUKF),该算法的核心思想是利用一组高斯分布的和去近似状态的后验概率密度,同时针对每一个高斯分布采用高阶无迹卡尔曼滤波算法进行估计.数值仿真实验结果表明,提出的HUT机制与普通的无迹变换(Unscented Transform,UT)相比,具有更高的近似精度;提出的GS-HUKF与传统的GSF以及高斯和粒子滤波器(Gaussian Sum Particle Filter,GS-PF)相比,兼容了二者的优点,即具有计算复杂度低和估计精度高的特性.%A novel high order unscented transform (HUT) mechanism is proposed to improve the approximation accuracy of the nonlinear transformation.The HUT is adopted to select the Sigma points which can be used to approximate the posterior probability density of state variable by numerical integration.Thus the high order unscented Kalman filtering (HUKF) algorithm can be made up.Further,to solve the state estimation problem for nonlinear/non-Gaussian system,Gaussian sum high order unscented Kalman filter (GS-HUKF) is proposed by combining the HUKF and Gaussian sum filter (GSF).The basic idea of the GS-HUKF is that a cluster of Gaussian distribution is used to approximate the posteriorprobability density of state variable.At the mean time,each separated Gaussian distribution is estimated by HUKF.Numerical simulation results demonstrate that the proposed HUT has higher estimation precision than ordinary unscented transform (UT) method.The GS-HUKF has integrated advantages with respect to estimation accuracy and computational complexity and its performance is superior to the existing Gaussian sum filters.【期刊名称】《电子学报》【年(卷),期】2017(045)002【总页数】7页(P424-430)【关键词】卡尔曼滤波;无迹卡尔曼滤波;高斯和;非线性非高斯【作者】王磊;程向红;李双喜【作者单位】安徽科技学院电气与电子工程学院,安徽蚌埠233100;东南大学仪器科学与工程学院,江苏南京210096;安徽科技学院电气与电子工程学院,安徽蚌埠233100【正文语种】中文【中图分类】TN713状态估计理论被广泛的应用于各领域,例如太空监测[1]、无线通信[2]、跟踪系统[3]以及金融行业[4]等.当系统为线性、噪声统计特性服从高斯分布并完全已知时,Kalman滤波器是最优的解决方案.然而,随着人们对系统认识的不断深化,以及估计和控制任务要求的日益提高,状态估计中非线性、非高斯的存在越来越成为不可回避的问题.传统的扩展卡尔曼滤波(Extended Kalman Filter,EKF)及其改进方法只能处理弱非线性系统及高斯噪声条件下的估计问题.为此,人们提出了一系列以贝叶斯滤波理论为框架、基于数值积分近似的非线性状态估计方法,其中具有代表性的方法包括中心差分卡尔曼滤波(Central Difference Kalman Filter,CDKF)[5]、无迹卡尔曼滤波(Unscented Kalman Filter,UKF)[6]、求积分卡尔曼滤波(Quadrature Kalman filter,QKF)[7]、容积卡尔曼滤波(Cubature Kalman Filter,CKF)[8]等.上述滤波器的共同特点是对状态的先验分布进行采样,通过对确定采样点进行独立处理和加权综合得到状态估计[9，10].EKF与确定采样型滤波算法均假设系统中噪声的统计特性是已知并服从高斯分布的,而在实际的非线性系统中,还存在着干扰信号统计特性不完全已知或不服从高斯分布的情况,例如在通讯、雷达、声纳、导航等领域,背景噪声往往非常复杂,存在非高斯的情况[11].假设系统噪声服从高斯分布的非线性滤波器对这一类问题是难以进行准确估计的,在实际系统中的应用受到限制.高斯和滤波器(Gaussian Sum Filter,GSF)常常被用于处理非线性、非高斯问题,其主要思想是通过一簇均值与方差不同的高斯分布可以近似任何概率密度函数,且近似的误差能任意小[12].基于蒙特卡罗方法的粒子滤波器(Particle Filter,PF)是另外一类能够用于处理非线性、非高斯滤波问题的方法,PF不受随机变量必须满足高斯分布的假设条件限制,需要的假设条件较少,但其计算复杂度太大,因此在实时性要求较高的一些应用例如导航、跟踪等领域中受到限制[13].普通的无迹变换(Unscented Transform,UT)对称采样策略能够保证高斯分布的近似精度达到泰勒级数展开式的三阶截断.为了进一步提高非线性变换的近似精度,Julier[14]提出了利用三阶矩信息的偏度采样策略,能够对任意分布近似达到三阶精度,但该方法比较复杂,很少被使用.Ponomareva[15]提出一种利用增广的办法来扩展传统的UT变换,可以捕获随机变量的4阶中心矩.Tenne[16]提出了一种采用5个对称分布的Sigma点能够捕获一维随机变量8阶中心矩.本文首先对近似非线性变换概率密度分布高阶矩的方法进行研究,提出了高斯和高阶无迹卡尔曼滤波算法.2.1 高阶UT变换事实上,所有的确定采样型滤波算法中,根据不同的采样策略选取的Sigma点与随机变量x的统计特性相关,数值积分的目的是采用尽可能少的Sigma点近似随机变量x中更多的高阶矩信息.由于多维高斯变量状态估计问题可以分解成单维高斯变量状态估计问题,因此,我们首先考虑x服从一维高斯分布的情形.假设一维随机变量x服从标准正态分布N(x;0,1),则其概率密度函数为设任意非线性函数g(x),其期望可以表示成如下形式其中σi和ωi分别为Sigma点以及对应的权值.从上式可以看出,对非线性函数g(x)在区间[-∞ ～+∞]上的积分可以近似为有限个Sigma点集合{ωi,σi}的加权和.由于g(·)为任意形式的非线性函数,因此,考虑根据随机变量x的概率密度函数计算Sigma点.下面将概率密度函数p(x)和它的导函数的极点作为Sigma点的情况进行分析,p(x)及其i阶导函数pi(x)(i≥0)的极点求取如下:由式(3)～(7)可以发现:p(x)函数自身有一个极点x=0,如果将它作为Sigma点,与把非线性函数在x=0处进行泰勒级数展开的EKF方法类似;一阶导函数p′(x)有两个对称的极点x=±1,与CKF滤波中一维高斯随机变量所选取的Sigma点相同;二阶导函数p″(x)的极点与UKF滤波中UT变换对称采样策略所得到的Sigma点一致.随机变量x概率密度函数的三阶导函数p(3)(x)的极点为四阶导函数p(4)(x)的极点为将这些极点在一维标准正态分布概率密度函数上进行标注,如图1所示.从图1可以看出,每组极值点均以x=0为中心对称分布,导函数的阶数越高,所获取的极值点越远离中心点.根据标准正态分布的统计特性,99.73%的点都分布在[-3,3]区间内,若某个极值点超出了这个范围,将它作为Sigma点所对应的权值将非常小,在计算积分的过程中可忽略.因此,对于极值点作为Sigma点的选择主要可根据计算复杂度和精度两个方面考虑,保证估计的准确性和实时性.下面介绍如何求得Sigma点对应的权值,首先介绍如下定理:定理1 假设一维随机变量x服从标准正态分布,其概率密度函数f(x)～N(0,Ω2),则x 的k阶中心距满足其中表示变量x的k阶中心矩.由于随机变量x服从标准正态分布,选择的Sigma点利用权重来描述其出现的概率大小,因此,引入权重信息后的Sigma点也应该具有和随机变量x相同的统计特性,二者的均值和高阶中心矩应该相同,即:其中为变量x的均值为引入权重信息的Sigma点的均值为变量x的k阶中心矩为引入权重信息Sigma点的k阶中心矩.引入权重信息Sigma点的均值和k阶中心矩可以表示如下:其中m为Sigma点的个数.由以上描述的Sigma点集合均以x=0为中心对称分布,且对称分布的Sigma点具有相同的权值,为便于描述,只选取非负的Sigma点σi(i=1,2,…γ,2γ+1=m),根据式(9)、(10),可得到将Sigma点代入式(11)进行计算,便能够得到相应的权值.通过这种方式可以求得与随机变量x高阶中心矩相匹配的Sigma点集合,利用获取的Sigma集合可以近似非线性变换后的高阶中心矩信息.多维随机变量Sigma点集合Si={ωi,ζi},(i=1,2,…m)可以通过一维标准正态分布获取的Sigma点进行扩展得到.假设n维随机变量x,其均值为协方差为Px,可以根据如下形式进行求取:其中,ζi表示多维随机变量第i个Sigma点,σi为服从标准正态分布的一维随机变量第i个非负的Sigma点,多维随机变量集合中各个Sigma点的权值ωi与一维随机变量Sigma点权值相同.HUT变换利用高斯概率密度函数及其导函数的极值点作为Sigma点,再根据式(11)计算得到相应权值,便能够通过少量的Sigma点捕获随机变量经过非线性变换后的高阶统计特征信息,与Ponomareva和Tenne的方法相比,该方法避免了直接对非线性等式的求解.2.2 高阶无迹卡尔曼滤波算法将HUT变换应用到非线性贝叶斯滤波结构中,可以得到HUKF算法,步骤如下:第一步时间更新(1)设k时刻随机变量x的先验概率密度函数已知,对Pk-1|k-1进行分解(2)通过HUT变换引入单维随机变量的Sigma点σi(i=1,2,…,m)(3)将引入Sigma点后的状态进行非线性传播(4)状态预测(5)协方差阵预测第二步量测更新(1)预测协方差分解(2)通过HUT变换引入Sigma点σi(i=1,2,…,m)(3)将引入Sigma点后的状态进行非线性传播(4)量测预测(5)预测新息协方差矩阵(6)预测互协方差矩阵(7)计算增益(8)状态估计(9)协方差估计3.1 高斯和定理将上述HUKF算法与高斯和滤波相结合,形成能够用于处理非线性、非高斯问题的高斯和高阶无迹卡尔曼滤波算法.定理2 对于某概率密度函数P(x),其近似表达Pm(x)可以表示成如下形式其中m为高斯分量的个数;ρi为第i个高斯分量对应的权重;N(x;ui,Σi)为高斯概率密度函数,均值为ui,协方差为Σi.文献[17]中对高斯和定理进行了详细论述,并指出随着高斯分量数目的不断增加,高斯混合结果能近似任意的非线性概率密度函数. 为了合理的选择高斯混合参数,例如权重ρi、均值ui和协方差Σi,设近似的概率密度函数Pm(x)与真实的非高斯概率密度函数P(x)的Ln范数为要使概率密度函数Pm(x)得到最优的近似效果,选择合理的参数使范数最小即可,一般选择n=2[18].3.2 高斯和确定采样型滤波根据高斯和定理,任何非高斯状态分布都可以通过若干个高斯分布以加权和的形式近似表示,且近似的误差可任意小.因此,对于过程噪声和量测噪声为加性非高斯噪声的系统,可以将非高斯分布表示成高斯概率密度叠加和的形式,再利用非线性贝叶斯滤波理论得到高斯和确定采样型滤波算法,该算法步骤如下:(1)初始化设k时刻加性过程噪声为wk,量测噪声为vk,由高斯和定理,先验噪声概率密度可以分别由如下形式表示其中,Ik和Jk分别为近似过程噪声与量测噪声所用高斯分量的个数;βki和μkj分别为各高斯分量的权值,并满足和初始时刻的先验状态概率密度也可表示为高斯和的形式其中,T0为近似先验状态概率密度所采用高斯分量个数,α0τ为各高斯分量对应的权值,满足同样,高斯和确定采样型滤波包括时间更新和量测更新两个过程,在每一次估计过程中,二者交替进行.(2)时间更新设k-1时刻的后验概率密度P(xk-1|z1:k-1)已知,并由高斯和定理表示为如下形式根据式(29)中的过程噪声,可以得到k时刻的状态一步转移概率密度P(xk|xk-1)利用贝叶斯递推公式,可以求得k时刻xk的状态概率密度估计式中,等号右边的积分项可以利用高斯确定采样型滤波中的时间更新步骤近似求得,从而得到概率密度函数P(xk|z1:k-1)其中α(k|k-1)τi=α(k|k-1)τβki至此,时间更新结束,从式(35)可以看出状态一步预测概率密度P(xk|z1:k-1)被Tk-1Ik项高斯分量表示.(3)量测更新根据式(30)所示的非高斯量测噪声形式,代入量测非线性方程中,得到似然概率密度P(zk|xk)当k时刻的量测量zk到来后,则可以利用高斯确定采样型滤波中的量测更新步骤将后验概率密度P(xk|z1:k)表示为高斯和形式其中其中最后,基于最小方差估计准则的状态估计及其协方差Pk|k可以由各个高斯分量中的状态估计和协方差进行加权求和得到由以上步骤可以看出,高斯分量的数目在时间更新阶段由Tk-1项增长到Tk-1Ik项,量测更新过程又增加到Tk-1IkJk项.随着算法运行时间的推移,高斯分量的数目会越来越多,导致算法实时性降低.因此,有必要对减少高斯分量数目的方法进行研究.事实上,在利用高斯和定理进行近似的过程中,某些高斯分量的权重,例如式(35)中的α(k|k-1)r,式(37)中的α(k|k)r与其它分量的权重相比非常小,对最终状态估计的结果影响甚微.因此,可以采用权重阈值比较的方法直接去掉权重非常小的高斯分量或将它们进行合并,减小由于对不必要分量计算而引起的实时性降低问题[19,20].下面通过用两个仿真实例分别对HUT机制和 GS-HUKF算法进行验证.(1)实例1:小球碰壁反弹模型小球碰壁反弹模型具有很强的非线性,在文献[6]、[16]中作为范例验证UT变换的性能,对该模型的描述如下:某弹性小球以初始速度v0=-1m/s撞向某一障碍物,碰撞到障碍物之后发生弹性形变,以相反的速度离开障碍物.记撞击时刻为0时刻,撞击点为原点,忽略除小球所受外力.小球与障碍物的距离y可以表示为初始距离x与时间t的函数设小球离障碍物的初始距离x服从标准正态分布,通过理论分析,随着时间变化的距离均值及其方差可以通过下面的表达式计算:采用普通的UT变换和HUT变换所得到的Sigma点进行近似,对小球离障碍物的距离均值及其方差Py进行估计,HUT变换所选用的Sigma点如表1所示.从表1可以看出,所选用的Sigma点均呈对称分布,其中4阶中心矩匹配HUT所采用的Sigma点、权值与普通的UT变换完全相同,它们最高可以捕获非线性变换后变量的前4阶中心距;8阶中心矩匹配HUT变换包括5个Sigma点,最高可以捕获非线性变换后的前8阶中心矩;20阶中心矩匹配HUT变换包括11个Sigma点,最高可以捕获非线性变换后的前20阶中心矩.将普通的UT、8阶HUT以及20阶HUT得到的Sigma点分别用于对弹性小球离障碍物的距离均值以及方差Py进行近似.近似得到的仿真结果如图2～图5所示,其中图2为小球离障碍物距离的估计曲线,图3为距离的估计误差;图4为小球离障碍物距离协方差的估计曲线,图5为协方差的估计误差.从图2～图5可以看出,通过普通UT、8阶HUT以及20阶HUT得到的Sigma点均能够近似出真实的距离均值与方差的变化,20阶HUT近似的结果精度最优,8阶HUT次之,普通的UT精度相对较低.由此可以看出,矩匹配的阶数越高,得到非线性变换估计结果的精度也相对较高.然而,随着Sigma点数的增多,计算复杂度也会随之增加.因此,如何合理的选用Sigma点,还需要根据实际的系统与需求确定.(2)实例2:单变量非平稳增长模型利用单变量非平稳增长模型(Univariate Nonstationary Growth Model,UNGM)验证GS-HUKF算法的性能,很多文献都对该模型进行了研究[21],其形式如下:,k=1,…,N其中,假设过程噪声服从Gamma分布,wk～Γ(3,2);量测噪声vk为服从标准正态分布的高斯白噪声.产生的参考数据初值x0=0.1,数据长度N=100.每次运行的初值x0从区间[0,1]中随机抽取,根据量测数据zk序列对状态xk进行估计,利用均方根误差对估计的结果进行评估将高斯和确定采样型滤波算法包括GS-HUKF、高斯和无迹卡尔曼滤波(Gaussian Sum Unscented Kalman Filter,GS-UKF)与高斯和求积分卡尔曼滤波(Gaussian Sum Quadrature Kalman filter,GS-QKF)进行了比较,由于粒子滤波估计精度较高,因此,也将GS-PF进行仿真,作为估计精度的参考.运行50次蒙特卡罗仿真的均方根误差曲线如图6所示.GS-HUKF中所选用的Sigma点按表1进行选取.GS-QKF算法中,QKF中选用的采样点数为3个.GS-PF中选用的是基于序贯重要性采样(Sequential Importance Resampling,SIR)的粒子滤波算法,其中粒子个数为300. 由图6中曲线可以看出,GS-UKF与GS-QKF估计误差较大,两种算法估计结果相近,这是由于Sigma点个数为3的QKF与普通对称采样策略UKF具有相同的估计精度水平.但是QKF受限于“维数诅咒”问题,在高维系统中的应用比较困难.在所有的算法中,GS-PF和GS-HUKF20跟踪精度较高,优于GS-UKF、GS-QKF和GS-HUKF8.表2统计了所有以上滤波算法运行50次蒙特卡罗仿真得到的均方根误差、误差方差以及运行时间.由表2可以看出,根据算法运行的时间对算法的计算复杂度进行评估,结合估计精度表现,可以得出如下结论:GS-HUKF计算复杂度高于GS-UKF,但处于同一个量级,而其估计精度比GS-UKF有大幅提高;GS-PF的估计精度与GS-HUKF20估计精度相当,但是其运行时间远远高于GS-HUKF.由此可以看出,GS-HUKF是一种能够较好兼顾计算复杂度和估计精度的非线性、非高斯滤波算法.本文对基于数值积分的采样点确定方法进行了研究,提出了HUT方法,通过该方法产生的Sigma点可近似随机变量非线性变换后概率密度分布的高阶矩,与普通UT 相比,HUT近似精度更高,将其应用到非线性贝叶斯滤波理论框架,得到了HUKF算法.进一步的,将HUKF与高斯和滤波理论相结合,得到一类能够用于处理非线性、非高斯问题的GS-HUKF算法,通过数值仿真对提出的算法进行了验证,与现有的高斯和算法相比,GS-HUKF能够较好兼顾计算复杂度和估计精度,证明了其优越性.王磊(通信作者) 男,1984年出生,博士、讲师,研究方向为非线性滤波理论与方法、多传感器信息融合技术.E-mail:*****************程向红女,1963年出生,博士、教授,研究方向为惯性导航技术、组合导航系统理论与方法.E-mail:***************.cn【相关文献】[1]Pei H L,Sanjeev A,Tharaka A L,Thushara D A.A Gaussian-Sum based cubature Kalman filter for bearings-only tracking[J].IEEE Transactions on Aerospace and Electronic Systems,2013,49(2):1161-1176.[2]Lim,K P.Ranking market efficiency for stock markets:A nonlinear perspective[J].Physica A:Statistical Mechanics and its Applications,2007,376(10):445-454.[3]Zhang H,Feng G,Han C. Linear estimation for random delay systems[J]. Systems and Control Letters,2011,60(7):450-459.[4]Li W,Jia Y. Distributed consensus filtering for discrete-time nonlinear systems with non-Gaussian noise[J].Signal Processing,2012,92(10):2464-2470.[5]Schei T S.A finite-difference method for linearization in nonlinear estimation algorithms[J].Automatica,1997,33(11):2053-2058.[6]Julier S J,et al.A new approach for filtering nonlinear systems[A].Proceedings of the American Control Conference[C].Seattle:IEEE,1995. 1628-1632.[7]Arasaratnam I,Haykin S,Elliott R J.Discrete-time nonlinear filtering algorithms using Gauss-Hermite quadrature[J].Proceedings of the IEEE,2007,95(5):953-977.[8]Arasaratnam I,Haykin S.Cubature kalman filters[J].IEEE Transactions on Automatic Control,2009,54(6):1254-1269.[9]Ito K,Xiong K.Gaussian filters for nonlinear filtering problems[J].IEEE Transactions on Automatic Control,2000,45(5):910-927.[10]Brunke S,Campbell M.Square root sigma point filtering for real-time,nonlinear estimation[J].Journal of Guidance,Control,and Dynamics,2004,27(2):314-317.[11]Li W,Jia Y.Distributed consensus filtering for discrete-time nonlinear systems with non-Gaussian noise[J].Signal Processing,2012,92(10):2464-2470.[12]Alspach D L,Sorenson H W.Nonlinear Bayesian estimation using Gaussian sum approximations[J].IEEE Transactions on Automatic Control,1972,17(4):439-448.[13]M Sanjeev Arulampalam,et al.A tutorial on particle filters for online nonlinear/non-Gaussian Bayesian tracking[J].IEEE Transactions on Signal Processing,2002,50(2):174-188.[14]Julier S J,et al.Unscented filtering and nonlinear estimation[J].Proceedings of the IEEE,2004,92(3):401-422.[15]Ponomareva K,Date P,Wang Z.A new unscented Kalman filter with higher order moment-matching[A].Proceedings of Mathematical Theory of Networks andSystems[C].Budapest: Eötvös Loránd University (ELTE),2010.1609-1613.[16]Tenne D,Singh T.The higher order unscented filter[A].Proceedings of the IEEE American Control Conference[C]. New York:IEEE,2003. 2441-2446.[17]Arasaratnam I,Haykin S,Elliott R J.Discrete-time nonlinear filtering algorithms using Gauss-Hermite quadrature[J].Proceedings of the IEEE,2007,95(5):953-977.[18]Caputi M,Moose R L.A modified Gaussian sum approach to estimation of non-Gaussian signals[J].IEEE Transactions on Aerospace and Electronic Systems,1993,29(2):446-451.[19]Clark J M C,Kountouriotis P A,Vinter R B.A Gaussian mixture filter for range-only tracking[J].IEEE Transactions on Automatic Control ,2011,56(3):602-613.[20]Horwood J T,Poore A B.Adaptive Gaussian sum filters for space surveillance[J].IEEE Transactions on Automatic Control,2011,56(8):1777-1790.[21]Wu Y,Hu D,Wu M,et al.A numerical-integration perspective on Gaussian filters[J].IEEE Transactions on Signal Processing,2006,54(8):2910-2921.。
无迹卡尔曼滤波算法
无迹卡尔曼滤波算法
无迹卡尔曼滤波算法（Unscented Kalman Filter，UKF）是一种用于处理非线性系统的非参数滤波算法，它可以从观测和测量数据中推断隐藏状态的值。
UKF的基本思想是基于状态变量的状态和测量变量的观测，使用一系列加权的状态估计来预测未来状态，并通过观测和测量值来校正预测值。
UKF的优点在于它可以处理非线性系统，而不需要对系统进行线性化处理，从而可以更准确地估计隐藏状态变量，准确度比传统卡尔曼滤波算法更高。
UKF是一种经典的非线性滤波算法，它可以利用观测和测量值，以及相关的不确定性信息，以准确的方式估计隐藏状态变量。
它也可以用于自适应控制，机器人移动控制，机器视觉，自动驾驶等领域。
UKF可以用来模拟复杂的物理过程，估计不同的系统参数，以及更准确地预测未来的状态，这在许多领域，如自动驾驶汽车，智能机器人，机器视觉，航空航天，大气科学和精细化工等领域中都很有用。
总之，无迹卡尔曼滤波算法是一种用于处理非线性系统的有效滤波算法，能够从观测和测量数据中推断隐藏状态的值，准确度比传统卡尔曼滤波算法更高，在航空航天，机器人，机器视觉，控制系统，大气科学和精细化工等领域都得到了广泛应用。
扩展卡尔曼滤波和无迹卡尔曼滤波
二、扩展Kalman滤波(EKF)算法
假定定位跟踪问题的非线性状态方程和测量方程如 下:
X k1 f ( X k ) Wk ...............(1)
Yk h( X k ) Vk ...................(.2)
在最近一次状态估计的时刻，对以上两式进
行线性化处理，首先构造如下2个矩阵:
%---------------------------------------
function UKFmain
%------------------清屏----------------
close all;clear all;
clc; tic;
global Qf n;
%定义全局变量
%------------------初始化--------------
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);
2n
Py wic (Yi y)(Yi y)T i0
................(9)
2n
Pxy wic (Yi x)(Yi y)T i0
..............(10)
由于x的均值和方差都精确到二阶，计算得到y 的均值和方差也精确到二阶，比线性化模型精度更 高。在卡尔曼框架内应用UT技术就得到了UKF算法。
r = sqrt(X(k,1)^2+X(k,3)^2) + dr*randn(1,1);
无迹卡尔曼滤波调参
无迹卡尔曼滤波调参无迹卡尔曼滤波（UKF）是一种常用的非线性滤波方法，用于估计系统的状态变量。
与传统的卡尔曼滤波不同，UKF通过引入一组离散采样点，以更好地逼近系统的非线性特性。
然而，UKF的性能很大程度上依赖于其参数的调整，下面将介绍一种调参方法。
在调参之前，我们首先需要了解UKF的几个关键参数。
首先是采样点的数量，通常选择与状态变量的维度相关的值。
其次是过程噪声和观测噪声的协方差矩阵，它们描述了系统中的不确定性和噪声。
最后是一个控制参数，称为增益参数，用于平衡系统的不确定性和噪声。
为了调整这些参数，我们可以采用试错的方法。
首先，选择一组初始参数，并使用这些参数运行UKF算法。
然后，根据滤波结果的性能评估参数的适用性。
如果滤波结果不理想，需要调整参数并再次运行算法。
调整参数时，可以采用以下策略。
首先，增加采样点的数量，以更好地逼近系统的非线性特性。
然后，根据系统的动态特性和噪声水平，适当调整过程噪声和观测噪声的协方差矩阵。
最后，通过调整增益参数，平衡系统的不确定性和噪声。
在调参过程中，需要注意以下几点。
首先，要根据具体应用场景和系统的特性，选择合适的参数范围。
其次，要根据滤波结果的性能，评估参数的适用性，并进行相应的调整。
最后，要注意参数之间的相互影响，避免过多调整导致系统性能的下降。
通过试错的方法和合理的参数选择，可以有效调参并提高无迹卡尔曼滤波算法的性能。
调参过程中，需要注意参数范围的选择，滤波结果的评估以及参数之间的相互影响。
通过不断优化参数，可以提高滤波算法的准确性和鲁棒性，从而更好地估计系统的状态变量。
几种卡尔曼滤波算法理论
几种卡尔曼滤波算法理论卡尔曼滤波（Kalman Filtering）是一种状态估计的方法，用于从不完全和带有噪声的观测数据中，估计出系统的状态。
它的基本思想是结合系统的动态模型和观测数据，通过最小化估计值与观测值之间的误差，实现对系统状态的准确估计。
以下是几种常见的卡尔曼滤波算法理论：1. 离散时间线性卡尔曼滤波（Discrete-Time Linear Kalman Filtering）：这是最基本、最常用的卡尔曼滤波算法。
它适用于系统的动态模型和观测模型均为线性的情况。
该算法基于状态方程和观测方程，通过递推的方式估计系统的状态。
2. 扩展卡尔曼滤波（Extended Kalman Filtering）：扩展卡尔曼滤波是一种非线性状态估计方法，用于处理非线性系统。
该算法通过在线性化非线性函数，将非线性系统转化为线性系统，然后应用离散时间线性卡尔曼滤波算法进行估计。
3. 无迹卡尔曼滤波（Unscented Kalman Filtering）：无迹卡尔曼滤波是对扩展卡尔曼滤波的改进。
与扩展卡尔曼滤波通过线性化非线性函数来估计系统状态不同，无迹卡尔曼滤波通过选择一组特殊的采样点（称为Sigma点），通过这些采样点的传播来逼近非线性函数的统计特性。
4. 无过程噪声卡尔曼滤波（Kalman Filtering with No Process Noise）：通常情况下，卡尔曼滤波算法假设系统的状态方程和观测方程中都存在噪声项，即过程噪声和观测噪声。
然而，在一些特殊的应用领域中，系统的状态方程并不包含过程噪声，只存在观测噪声。
无过程噪声卡尔曼滤波算法就是针对这种情况设计的。
5. 卡尔曼平滑（Kalman Smoothing）：卡尔曼滤波算法是一种递推算法，只使用当前的观测数据和先前的状态估计，来估计当前的状态。
而卡尔曼平滑算法则是一种回溯算法，根据所有的观测数据来获得更优的对过去状态的估计。
卡尔曼平滑算法一般通过前向-后向过程来实现。
无迹卡尔曼滤波 指数模型-概述说明以及解释
无迹卡尔曼滤波指数模型-概述说明以及解释1.引言1.1 概述无迹卡尔曼滤波（UKF）是一种用于非线性系统状态估计的先进滤波算法，通过在高斯分布上取样一些特定点（称为无迹变换），对系统状态的不确定性进行建模。
相比传统的卡尔曼滤波算法，UKF能够更好地处理非线性系统，并且在一些情况下具有更好的性能。
指数模型是一种常见的数学模型，通常用于描述随时间指数增长或减少的趋势。
指数模型在经济学、生物学、工程学等领域中被广泛应用，可以帮助分析和预测未来的趋势变化。
本文将介绍无迹卡尔曼滤波和指数模型的基本原理和应用方法，同时探讨它们在现实生活中的实际应用案例。
通过深入理解这两种模型，我们可以更好地应用它们解决现实世界中的问题，提高系统的状态估计精度和预测准确性。
1.2 文章结构本文主要分为三个部分，即引言、正文和结论。
具体如下：引言部分主要介绍了本文的背景和意义，框定了研究的范围和目的，为读者提供了对整个文章内容的整体认识。
正文部分包括了对无迹卡尔曼滤波和指数模型的介绍，分别讨论了它们的定义、原理和特点，同时给出了相关的数学公式和算法，通过实例分析展示了它们的应用。
结论部分对整篇文章进行了总结，重点强调了无迹卡尔曼滤波和指数模型在实际应用中的价值和意义，并展望了它们的未来发展方向。
同时给出了本文的结论和建议，为读者提供了在相关领域深入研究的参考。
1.3 目的本文旨在探讨无迹卡尔曼滤波和指数模型在数据处理和预测中的应用。
通过分析这两种算法的原理和特点，我们将更深入地了解它们在实际情况下的适用性和效果。
同时，我们也将通过实际案例的介绍来展示这两种算法在不同领域的应用效果，从而为读者提供更多关于无迹卡尔曼滤波和指数模型的了解和应用方法。
我们希望通过本文的阐述，读者能够对这两种算法有一个全面的认识，并在实际工作中灵活运用它们以解决问题和提升工作效率。
2.正文2.1 无迹卡尔曼滤波无迹卡尔曼滤波（UKF）是一种基于传统卡尔曼滤波算法的扩展，旨在解决非线性系统状态估计问题。
ukf（无迹卡尔曼滤波）算法的matlab程序
ukf（⽆迹卡尔曼滤波）算法的matlab程序转载⾃：https:///ss19890125/article/details/32121969#0-tsina-1-16645-397232819ff9a47a7b7e80a40613cfe1function [x,P]=ukf(fstate,x,P,hmeas,z,Q,R)% UKF Unscented Kalman Filter for nonlinear dynamic systems% [x, P] = ukf(f,x,P,h,z,Q,R) returns state estimate, x and state covariance, P% for nonlinear dynamic system (for simplicity, noises are assumed as additive):% x_k+1 = f(x_k) + w_k% z_k = h(x_k) + v_k% where w ~ N(0,Q) meaning w is gaussian noise with covariance Q% v ~ N(0,R) meaning v is gaussian noise with covariance R% Inputs: f: function handle for f(x)% x: "a priori" state estimate% P: "a priori" estimated state covariance% h: fanction handle for h(x)% z: current measurement% Q: process noise covariance% R: measurement noise covariance% Output: x: "a posteriori" state estimate% P: "a posteriori" state covariance%% Example:%{n=3; %number of stateq=0.1; %std of processr=0.1; %std of measurementQ=q^2*eye(n); % covariance of processR=r^2; % covariance of measurementf=@(x)[x(2);x(3);0.05*x(1)*(x(2)+x(3))]; % nonlinear state equationsh=@(x)x(1); % measurement equations=[0;0;1]; % initial statex=s+q*randn(3,1); %initial state % initial state with noiseP = eye(n); % initial state covraianceN=20; % total dynamic stepsxV = zeros(n,N); %estmate % allocate memorysV = zeros(n,N); %actualzV = zeros(1,N);for k=1:Nz = h(s) + r*randn; % measurmentssV(:,k)= s; % save actual statezV(k) = z; % save measurment[x, P] = ukf(f,x,P,h,z,Q,R); % ekfxV(:,k) = x; % save estimates = f(s) + q*randn(3,1); % update processendfor k=1:3 % plot resultssubplot(3,1,k)plot(1:N, sV(k,:), '-', 1:N, xV(k,:), '--')end%}%% By Yi Cao at Cranfield University, 04/01/2008%L=numel(x); %numer of statesm=numel(z); %numer of measurementsalpha=1e-3; %default, tunableki=0; %default, tunablebeta=2; %default, tunablelambda=alpha^2*(L+ki)-L; %scaling factorc=L+lambda; %scaling factorWm=[lambda/c 0.5/c+zeros(1,2*L)]; %weights for meansWc=Wm;Wc(1)=Wc(1)+(1-alpha^2+beta); %weights for covariancec=sqrt(c);X=sigmas(x,P,c); %sigma points around x[x1,X1,P1,X2]=ut(fstate,X,Wm,Wc,L,Q); %unscented transformation of process% X1=sigmas(x1,P1,c); %sigma points around x1% X2=X1-x1(:,ones(1,size(X1,2))); %deviation of X1[z1,Z1,P2,Z2]=ut(hmeas,X1,Wm,Wc,m,R); %unscented transformation of measurments P12=X2*diag(Wc)*Z2'; %transformed cross-covarianceK=P12*inv(P2);x=x1+K*(z-z1); %state updateP=P1-K*P12'; %covariance updatefunction [y,Y,P,Y1]=ut(f,X,Wm,Wc,n,R)%Unscented Transformation%Input:% f: nonlinear map% X: sigma points% Wm: weights for mean% Wc: weights for covraiance% n: numer of outputs of f% R: additive covariance%Output:% y: transformed mean% Y: transformed smapling points% P: transformed covariance% Y1: transformed deviationsL=size(X,2);y=zeros(n,1);Y=zeros(n,L);for k=1:LY(:,k)=f(X(:,k));y=y+Wm(k)*Y(:,k);endY1=Y-y(:,ones(1,L));P=Y1*diag(Wc)*Y1'+R;function X=sigmas(x,P,c)%Sigma points around reference point%Inputs:% x: reference point% P: covariance% c: coefficient%Output:% X: Sigma pointsA = c*chol(P)';Y = x(:,ones(1,numel(x)));X = [x Y+A Y-A];。
无迹卡尔曼滤波算法在污水处理BSM1过程中的应用研究
无迹卡尔曼滤波算法在污水处理BSM1过程中的应用研究无迹卡尔曼滤波算法在污水处理BSM1过程中的应用研究摘要：污水处理是现代城市生活中不可或缺的环节，有效地监测和控制污水处理过程对于保护环境和人类健康至关重要。
本文针对污水处理中的BSM1过程，研究了无迹卡尔曼滤波算法在该过程中的应用。
1. 引言污水处理是将城市生活中产生的废水经过一系列物理、化学和生物处理过程，将有害物质转化为无害物质的过程。
污水处理的目标是确保出水质量符合排放标准，以保护环境和人类健康。
BSM1（Biofilm Sequencing Batch Reactor Model Number One）是一种常用的污水处理过程模型，广泛用于实际污水处理厂的操作和控制。
2. 无迹卡尔曼滤波算法简介无迹卡尔曼滤波算法是一种用于估计状态变量的算法，常用于系统辨识和控制中。
它与传统的卡尔曼滤波算法相比，能够具备更好的非线性系统估计性能。
无迹卡尔曼滤波算法通过引入一组称为无迹变换的技术，从而避免了传统卡尔曼滤波算法中的线性化假设。
3. BSM1过程的建模与控制BSM1过程模型涉及生物膜反应器中的多个变量，例如有机物质浓度、氨氮浓度等，这些变量之间存在复杂的非线性关系。
传统的控制方法往往无法应对这种非线性特性，因此需要引入更高级的算法进行控制。
无迹卡尔曼滤波算法在这种情况下具有应用前景。
4. 无迹卡尔曼滤波算法在BSM1过程中的应用本文基于BSM1过程的模型，设计了一个基于无迹卡尔曼滤波算法的控制系统。
首先，通过实际污水处理厂的数据，构建了一个准确的BSM1模型，并用于算法的验证。
然后，将无迹卡尔曼滤波算法应用于模型中的状态估计，以获取实时的变量状态信息。
最后，通过与传统的控制方法进行对比实验，验证了无迹卡尔曼滤波算法在BSM1过程中的优越性。
5. 结果与讨论实验结果表明，基于无迹卡尔曼滤波算法的控制系统在BSM1过程中具有较高的稳定性和控制精度。
无迹卡尔曼滤波算法公式
无迹卡尔曼滤波算法（UKF）的公式主要包括以下几个步骤：
1. 权重和方差计算公式：这个步骤主要是计算Sigma点的权重和方差。
2. Sigma点传播：根据状态方程计算x的预测值和协方差矩阵。
3. 得到一组新的Sigma点：根据上一步得到的预测值和协方差矩阵，得到一组新的Sigma点。
4. 代入观测方程中，得到测量量的预估值：将上一步得到的Sigma点代入观测方程中，得到测量量的预估值。
5. 获得观测量的预测值和协方差矩阵：根据上一步得到的预估值和协方差矩阵，计算观测量的预测值和协方差矩阵。
6. 更新状态变量和协方差矩阵：根据模型估计与测量估计的数据融合公式，更新状态变量和协方差矩阵。
以上步骤就是无迹卡尔曼滤波算法的公式，希望对解决您的问题有所帮助。
无迹卡尔曼滤波算法
%该文件用于编写无迹卡尔曼滤波算法及其测试％注解:主要子程序包括：轨迹发生器、系统方程％测量方程、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',。
- 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。