UKF算法范文

UKF算法范文（精选6篇）

UKF算法 第1篇

20世纪90年代以来,随着汽车电子控制技术的飞速发展,准确实时获取汽车行驶过程中的状态信息成为汽车动态控制系统研究的关键问题,由此而衍生出的汽车状态估计器的设计逐渐成为近年来研究的热点[1]。汽车行驶过程中并不是所有的状态量都可以直接测量的,一般说来,汽车状态估计的对象都是一些控制逻辑参量,这些参量难以测量但对于汽车电子控制系统又十分重要,例如汽车侧偏角等。还有一些状态量尽管可以通过仪器直接测量,但通过状态估计可以在很大程度上消除信号中的量测噪声和系统过程噪声等干扰。

目前,基于卡尔曼滤波算法的估计器是较为常用的一种汽车状态估计器,其中的常规卡尔曼滤波[2]采用线性汽车动力学模型进行状态估计。扩展卡尔曼滤波[3](extended Kalman filter,EKF)及其改进算法[4,5]则可以对包含非线性因素的汽车模型进行状态估计。扩展卡尔曼滤波算法在运算时需要求解繁琐的Jacobian矩阵,不但容易出错而且导致估计器软件的实时性降低。

本文把目标跟踪领域较为流行的unscented卡尔曼滤波(unscented kalman filter,UKF)算法应用在汽车的状态估计之中,针对各种汽车动态控制系统的不同控制逻辑要求,对汽车进行多状态估计。

1 非线性汽车动力学模型

1.1 整车模型

本文提出的估计算法基于图1所示的七自由度非线性汽车动力学模型,这里的7个自由度包括纵向、侧向、横摆和4个车轮的回转运动。

汽车的运动微分方程如下:

纵向

式中,vx为纵向速度;vy为侧向速度;ax为纵向加速度;ω为横摆角速度;δ为前轮转角;m为整车质量。

侧向

式中,ay为侧向加速度。

横摆

式中,Γ为整车绕z轴的横摆力矩;tf、tr分别为前轮距宽和后轮距宽;Fxij为各个车轮上的纵向力;Fyij为各个车轮上的侧向力;Mzij为各个车轮上的回正力矩;Iz为整车绕铅垂轴的转动惯量;下标f、r、L、R分别表示前轮、后轮、左轮、右轮。

忽略惯性阻力偶矩和空气升力作用,汽车右转弯时各个轮胎的垂直载荷为

$\begin{array}{l}F{}_{z\text{f}\text{L}}=b(mg/2+ma{}_{y}h/t{}_{\text{f}})/l-ma{}_{x}h/(2l)\\ F{}_{z\text{f}\text{R}}=b(mg/2-ma{}_{y}h/t{}_{\text{f}})/l-ma{}_{x}h/(2l)\\ F{}_{z\text{r}\text{L}}=a(mg/2+ma{}_{y}h/t{}_{\text{r}})/l+ma{}_{x}h/(2l)\\ F{}_{z\text{r}\text{R}}=a(mg/2-ma{}_{y}h/t{}_{\text{r}})/l+ma{}_{x}h/(2l)\end{array}\}(7)$

式中,a、b为整车质心至前后轴的距离,l=a+b;l为前后轴的距离;g为重力加速度;h为质心高度。

各个轮胎的侧偏角可通过下式求得[6]

$\begin{array}{l}\alpha {}_{\text{f}\text{L}}=\delta -\arctan \frac{v{}_y+a\omega }{v{}_x+t{}_{\text{f}}\omega /2}\\ \alpha {}_{\text{f}\text{R}}=\delta -\arctan \frac{v{}_y+a\omega }{v{}_x-t{}_{\text{f}}\omega /2}\\ \alpha {}_{\text{r}\text{L}}=-\arctan \frac{v{}_y-b\omega }{v{}_x+t{}_{\text{r}}\omega /2}\\ \alpha {}_{\text{r}\text{R}}=-\arctan \frac{v{}_y-b\omega }{v{}_x-t{}_{\text{r}}\omega /2}\end{array}\}(8)$

整车质心侧偏角为

β=arctan(vy/vx) (9)

各个轮胎的滑移率为

Sij=(reωij-uwij)/uwij (10)

式中,re为轮胎滚动半径;ωij为车轮转动角速度;uwij为车轮中心速度。

各车轮的车轮中心速度uwij可通过下式求得[7]:

$\begin{array}{l}u{}_{\text{w}\text{f}\text{L}}=v{}_{\text{c}\text{o}\text{g}}+\omega (t{}_{\text{f}}/2+a\beta )\\ u{}_{\text{w}\text{f}\text{R}}=v{}_{\text{c}\text{o}\text{g}}-\omega (t{}_{\text{f}}/2-a\beta )\\ u{}_{\text{w}\text{r}\text{L}}=v{}_{\text{c}\text{o}\text{g}}+\omega (t{}_{\text{r}}/2-b\beta )\\ u{}_{\text{w}\text{r}\text{R}}=v{}_{\text{c}\text{o}\text{g}}-\omega (t{}_{\text{r}}/2+b\beta )\end{array}\}(11)$

式中,vcog为质心速度。

1.2 轮胎模型

对于轮胎的非线性侧偏特性的描述本文采用修正Fiala模型[8]:

式中,Cs为线性域下轮胎的纵向滑移刚度,Cs=|∂Fx/∂S|s=0;θ为量纲一侧偏角,且θ=Cαtanα/(μ|Fz|);μ为路面附着系数;Cα为线性域下轮胎的侧偏刚度,Cα=|∂Fy/∂α|α=0;φ为路面附着因数;Fz为轮胎垂向载荷;Lr为轮胎印迹长度(假设印迹为长方形);e为轮胎拖距。

本文中取Cs=1200N,Cα=600N/(°),μ=1.0,Lr=70mm,e=31.7mm。

1.3 包含噪声的非线性汽车系统

非线性汽车系统的状态向量设为

xs=(vx,vy,ax,ay,ω,β,Γ)T (15)

系统输入为

u=(δ,ωfL,ωfR,ωrL,ωrR)T (16)

观测向量为

y=(ω,ay,vx) (17)

状态估计的过程噪声设为均值为零的时不变的平稳高斯白噪声序列,方差阵为对角矩阵,对角线元素按如下规律设置:

Q=0.01(max xi-min xi)

i=1,2,,7

其中,xi为xs中的第i个状态量,时间历程为10s。

量测噪声协方差矩阵设为均值为零的时不变的平稳高斯白噪声序列,方差阵为对角矩阵,对角线元素按如下规律设置:

其中,yj为y中的第j个状态量,时间历程为10s。

2 unscented卡尔曼滤波

unscented卡尔曼滤波是不同于扩展卡尔曼滤波的另外一类非线性估计方法。与EKF不同,UKF不对非线性模型做近似,而对状态的概率密度函数做近似。该滤波算法以unscented变换为基础,采用卡尔曼线性滤波框架,采样形式为确定性采样。

将UKF算法运用到行驶汽车的状态估计中,选择合适的采样策略和比例修正算法,推导UKF算法的具体步骤。

2.1 Sigma点的对称采样策略和比例修正算法

本文采用对称采样策略[9]选取Sigma点。对于均值为$\overline{\mathit{x}}$、方差为Px的n维随机变量,产生一个χ矩阵,该矩阵由2n+1个χk列向量组成:

$\mathit{\chi }{}_k=\{\begin{array}{l}\overline{\mathit{x}}k=0\\ \overline{\mathit{x}}+(\sqrt{(n+\lambda )\mathit{{\rm P}}{}_x}){}_{k}k=1,2,\cdots ,n\\ \overline{\mathit{x}}-(\sqrt{(n+\lambda )\mathit{{\rm P}}{}_x}){}_{k-n}k=n+1,n+2,\cdots ,2n\end{array}(18)$

为了消除采样的非局部效应,对原始Sigma点集进行比例修正[10],各个Sigma点的权值为

$W{}_i^{\text{m}}=\{\begin{array}{l}\lambda /(n+\lambda )i=0\\ 1/[2(n+\lambda )]i=1,2,\cdots ,2n\end{array}(19)$

$W{}_i^{\text{c}}=\{\begin{array}{l}\lambda /(n+\lambda )+1-\xi {}^2+\gamma i=0\\ 1/[2(n+\lambda )]i=1,2,\cdots ,2n\end{array}(20)$

其中,Wmi为均值的权;Wci为方差的权。λ为调节参数,λ=ξ2(n+κ)-n;κ≥0以保证方差矩阵的半正定性,本文选择κ=0。ξ为一个比例缩放因子,表示Sigma点围绕$\overline{\mathit{x}}$的延伸,可通过调整ξ的取值来调整Sigma点与$\overline{\mathit{x}}$的距离,这个值通常被设为一个小的正值(10-4ξ1)[11],采用的修正Fiala模型中轮胎力和力矩对于轮胎侧偏角的传递关系是正切函数和指数函数相互耦合的情况,表现出较强的非线性,因此为了防止非局部采样效应,选取ξ=0.01。γ包含x先验分布的高阶矩信息,对于高斯分布,γ=2是最优的[11]。

2.2 算法步骤

本文采用非扩展形式的UKF算法,假设前文中的非线性汽车系统的状态噪声和观测噪声均为高斯白噪声,方差阵分别为Q和R,则整个滤波算法步骤如下[11]:

(1)初始均值和方差分别为

x0=E(x0) (21)

$\mathit{{\rm P}}{}_0=E((\mathit{x}{}_0-\overline{\mathit{x}})(\mathit{x}{}_0-\overline{\mathit{x}}){}^{\text{Τ}})(22)$

为了确保算法收敛,初始误差方差阵P0设为对角元素为1的七阶稀疏方阵。

(2)计算Sigma点。根据式(18)产生2n+1个Sigma点(列向量)组成的列向量χk(k=0,1,2,,2n)

$\mathit{\chi }{}_k=(\mathit{x}{}_{k|k},\mathit{x}{}_{k|k}+\sqrt{(n+\lambda )\mathit{{\rm P}}{}_{k|k}},\mathit{x}{}_{k|k}-\sqrt{(n+\lambda )\mathit{{\rm P}}{}_{k|k}})(23)$

(3)时间更新过程。由1.1节中的非线性系统状态方程对各个Sigma点进行非线性变换:

通过加权得到状态的一步预测值:

$\mathit{x}{}_{k+1|k}={\displaystyle \sum _{i=0}^{2n}(}W{}_i^{\text{m}}\mathit{\chi }{}_{i,k+1|k})(25)$

通过加权得到方差阵的一步预测值:

$\mathit{{\rm P}}{}_{k+1|k}={\displaystyle \sum _{i=0}^{2n}[}W{}_i^{\text{c}}(\mathit{\chi }{}_{i,k+1|k}-\mathit{x}{}_{k+1|k})(\mathit{\chi }{}_{i,k+1|k}-\mathit{x}{}_{k+1|k}){}^{\text{Τ}}]+\mathit{Q}(26)$

由观测方程对各Sigma点进行非线性变换:

通过加权得到系统的一步预测观测值:

$\mathit{y}{}_{k+1|k}={\displaystyle \sum _{i=0}^{2n}(}W{}_i^{\text{m}}\mathit{\psi }{}_{i,k+1|k})(28)$

(4)量测更新。求系统输出的方差阵:

$\begin{array}{l}\mathit{{\rm P}}{}_{y{}_{k+1}y{}_{k+1}}=\\ {\displaystyle \sum _{i=0}^{2n}[}W{}_i^{\text{c}}(\mathit{\psi }{}_{i,k+1|k}-\mathit{y}{}_{k+1|k})(\mathit{\psi }{}_{i,k+1|k}-\mathit{y}{}_{k+1|k}){}^{\text{Τ}}]+\mathit{R}(29)\end{array}$

计算协方差阵

$\begin{array}{l}\mathit{{\rm P}}{}_{x{}_{k+1}y{}_{k+1}}=\\ {\displaystyle \sum _{i=0}^{2n}[}W{}_i^{\text{c}}(\mathit{\chi }{}_{i,k+1|k}-\mathit{x}{}_{k+1|k})(\mathit{\psi }{}_{i,k+1|k}-\mathit{y}{}_{k+1|k}){}^{\text{Τ}}](30)\end{array}$

计算滤波增益阵:

Kk+1=Pxk+1yk+1P${}_{\mathit{y}{}_{\mathit{k}+1}\mathit{y}{}_{\mathit{k}+1}}^{-1}$ (31)

得到状态更新后的滤波值:

xk+1|k+1=xk+1|k+Kk+1(yk+1-yk+1|k) (32)

求解状态后验方差阵:

Pk+1|k+1=Pk+1|k-Kk+1Pyk+1yk+1KTk+1 (33)

式(23)～式(33)中的权值按照式(19)、式(20)来确定。

3 基于ADAMS的虚拟试验验证

为了验证提出算法的正确性,在虚拟样机软件ADAMS上对某型轿车进行了仿真试验验证。该车的参数为:m=1528kg,a=1.48m,b=1.08m,h=0.432m,Iz=2440kgm2,tf=1.520m,tr=1.594m,re=0.33m。

整车虚拟样机模型分成前后悬架、车身、转向系、制动系以及前后车轮等子系统,分别建立相应子系统模型;建立各子系统之间以及子系统与ADAMS/Car提供的实验台之间相互交换信息的输入、输出信号器Communicator;轮胎模型采用ADAMS/Car中自带的Fiala轮胎模型。按系统组装成整车虚拟样机试验模型,如图2所示。

为了模拟极限工况下汽车的操纵响应,本文在整车仿真中采用驱动样机产生所需要的汽车行驶路径,使汽车沿图3所示的蛇行线路径行驶。整个操纵历时10s,采样时间为0.01s。

为了考察UKF算法对含噪声非线性汽车系统的估计性能,本文对UKF算法和EKF算法进行了比较。图4是UKF、EKF两种算法对vx、vy、ax、ay、ω、β这6个关键状态量的估计值与虚拟试验值的比较。

从图4可以看出,两种算法的估计值非常接近,纵向速度、纵向加速度及侧向加速度峰值处的估计EKF算法估计值的精度不如UKF算法估计值的精度。各个状态量的估计值在波峰和波谷上的估计值和虚拟试验值的误差较其他地方大一些,这是由于轮胎侧偏特性已进入较为明显的非线性区域。提高轮胎力学模型精度可以改善估计时状态量的跟随性能。

总体来说6个状态量的估计值与虚拟试验值的吻合程度较好,下面给出两种估计算法的误差指标,如表1所示。为了定量的比较两种算法的估计精度,给出了估计值相对于实际值的平均绝对误差和均方根误差,如表1所示。

从表1可以看出在同等条件下,UKF算法的估计精度要略高于EKF算法的估计精度,这也佐证了文献[12]中“UF可以准确估计均值达到Taylor级数的四阶精度,而EKF估计均值达到二阶精度”的结论。

同时比较了两种算法程序在奔腾四微机(CPU 2.4GHz,内存1GB)上的运行时间,在同等环境下UKF算法的运行时间是4.125s,EKF算法的运行时间是7.653s。由此可见虽然二者计算量同阶[13]。MATLAB环境下由于EKF算法需要通过符号运算来计算繁琐的Jacobian矩阵,因此占用了不少运行时间。如果本文程序通过Dspace快速原型控制开发系统直接用于硬件实时控制,则UKF方法的实时性要好一些。

综上,在估计精度和实时性方面UKF算法相比于目前广泛使用的EKF算法具有优势,完全可以满足汽车状态估计器对于软件性能的要求。

4 实车试验验证

进行某轻型越野汽车的蛇行线场地实车试验,试验按照GB/T 6323.1-1997标准执行。车上安装了陀螺仪用来实时采集汽车的横摆角速度和侧向加速度,用非接触式速度传感器用来实测汽车的纵向速度和侧向速度,用ABS轮速传感器采集4个车轮的角速度,用方向盘转角测试仪测量方向盘转角。试验车速为65km/h(±3km/h)。图5～图7所示为vy、ay、ω这3个状态量的UKF方法估计值和实车试验值的比较。

从以上的比较可以看出,虽然估计值与虚拟实验值存在一定的误差,但估计值在趋势上基本保持了与试验值的一致。本文采用的Fiala轮胎模型在模拟实车轮胎力学特性时还存在一定的偏差,另外传感器的测量误差及安装位置也是造成估计值与试验值有一定偏差的重要原因。

5 结论

(1)采用的UKF算法对于包含纯加性噪声的非线性汽车系统具有良好的状态估计精度,在ADAMS虚拟试验的快速蛇行极限工况下,状态估计的平均绝对误差都在状态幅值的10%以内。

(2)在同等条件下UKF算法的估计精度略高于常用的EKF算法,而在MATLAB环境下所花费的时间UKF算法大约只有EKF算法的50%。这说明在精度和实时性方面UKF算法完全可以胜任汽车状态估计器对于软件性能的要求。

摘要：准确实时获取行驶过程中的状态信息是汽车动态控制系统研究的关键问题。将unscented卡尔曼滤波(UKF)算法应用到汽车的状态估计之中,建立了包含时不变统计特性噪声和非线性轮胎的汽车动力学模型,采用具有对称采样策略和比例修正的UKF算法对汽车估计了多个关键状态量。将UKF估计器与常见的EKF估计器进行了比较分析,基于ADAMS/Car的虚拟试验和实车试验验证了UKF在汽车状态估计中的可行性。

UKF算法 第2篇

基于地磁场测量估计卫星姿态的UKF算法

提出了利用UKF(Unscented Kalman Filter)处理地磁场测量数据进行低轨道(LEO)卫星自主定姿的算法.通过使用估计姿态、轨道参数和国际地磁场参考(IGRF)计算得到的地磁矢量与三轴磁强计(TAM)的测量矢量之差作为更新信息,可以实现实时的姿态角和角速度估计.针对卫星稳态定姿、大角度快速机动的定姿以及姿态失控状态下的.定姿等三种任务,分别用UKF和传统的EKF(Extended Kalman Filter)进行了数值仿真.仿真结果显示出本文提出的定姿算法的优越性.

作 者：朱建丰 徐世杰 ZHU Jian-feng XU Shi-jie 作者单位：北京航空航天大学,宇航学院504教研室,北京,100083刊 名：宇航学报 ISTIC PKU英文刊名：JOURNAL OF ASTRONAUTICS年，卷(期)：27(6)分类号：V412.4关键词：姿态确定 UKF 磁强计

UKF算法 第3篇

编队构型的保持和控制需要精确确定航天器间的相对位置和相对速度, 即相对导航[2]。目前航天器近距离相对状态的确定常采用C-W方程 (即Hill方程) , 而在一般情况下航天器间的相对运动是非线性的。编队的精确导航需要引入各种相对状态测量装置进行相对距离、速度、视线角度等观测测量, 观测测量与滤波状态之间所具有的非线性加剧了滤波的非线性性。目前对于非线性模型的状态估计有Unscented Kalman Filter (UKF) 和先进的粒子滤波技术等。UKF采用粒子滤波的采样思路, 通过仔细选择的高斯随机变量采样点最小集合来近似系统的估计状态, 这些采样点具有高斯随机变量 (状态) 的均值和方差, 所以当通过非线性方程进行演化后它们以二阶精度近似非线性系统状态的验后分布。文中基于UKF滤波技术进行多航天器编队飞行相对导航研究并进行了仿真, 对仿真结果进行了分析。

1 UKF滤波方法简介

对于非线性系统的描述, 这里不失一般性, 进行下述表示[3]

式中, $\mathit{x}{}_i\widehat{{\rm I}}\mathit{R}{}^n$表示系统状态量, $\mathit{v}{}_k\widehat{{\rm I}}\mathit{R}{}^q$表示系统过程噪声, $\mathit{y}{}_k\widehat{{\rm I}}\mathit{R}{}^p$表示系统输出, $\mathit{w}{}_k\widehat{{\rm I}}\mathit{R}{}^r$表示系统观测噪声。

对于UKF, 这里采用如式 (1) 所示的系统状态描述[3], 其滤波算法描述如下:

(1) 初始化。

若过程噪声v和量测噪声w具有可加性, X是Sigma点向量, P0是非线性状态向量的初始协方差, Q和R分别是非线性部分过程噪声协方差和量测协方差矩阵。设置初始值如下

$\widehat{\mathit{x}}{}_0=\text{E}(\mathit{x}{}_0),{\rm P}{}_0=\text{E}[\mathit{x}{}_0-\widehat{\mathit{x}}{}_0(\mathit{x}{}_0-\widehat{\mathit{x}}{}_0){}^{\text{Τ}}]$ (2)

(2) 计算Sigma点。

式中, l=a2 (n+k) -n, 常数a用于确定Sigma点在其均值附近的分布情况, 往往给定一个小的正值, 针对不同的非线性状态配置不同的l值将有助于状态滤波的收敛以及滤波精度的提高, k通常被置为零, b包含x分布的先验信息, 通常被置为一个小的正值, $(\sqrt{(n+l){\rm P}{}_{k-1}}){}_i$是矩阵平方根的第i列, W${}_i^m$, W${}_i^c$是均值和协方差阵的权值, 上标m和c分别表示均值和协方差。

(3) 非线性状态的预测与更新。

预测:

更新:

2 基于CDGPS的编队飞行相对导航

2.1 系统状态方程

如图1所示, 定义相对轨道位置向量为ρ=[x, y, z]T, 相对速度向量为$\dot{\mathit{\rho }}=[\dot{x},\dot{y},\dot{z}]{}^{\text{Τ}}$。相对轨道运动学方程为[4]

式中, p表示主星轨道半通径, rc表示主星轨道地心距, $\dot{q}$表示主星真近点角速度, vx, vy和vz表示相对运动扰动项, 采用零均值高斯白噪声近似表示。真近点角加速度和主星轨道半径加速度为

若主星轨道为圆轨道, 则$\dot{r}=0,p=r{}_c$, 上述相对轨道运动学方程式 (6) 简化为著名的Hill方程。近圆参考轨道的线性相对动力学方程HCW (Hill-Clohessy-Wiltshire) 方程为

其中$n=\dot{q},$表示平运动。

2.2 系统观测方程

GPS载波相位测量方程为[5]

$f{}_i^j+l{\rm N}{}_i^j=r{}_i^j-\dot{r}{}_i^j\text{d}t{}_i+C(\text{d}t{}_i-\text{d}t{}^j)-{\rm I}{}_i^j+b{}_i^j+b{}_i^j+W{}_i^j$ (9)

式中, fji是载波相位小数部分测量值, N${}_i^j$是初始整周模糊数, l是载波波长, r${}_i^j$是第i颗编队星到第j颗导航星的几何距离, ti、tj是接收机与导航星钟差, C是光速, I${}_i^j$是电离层延迟项, b${}_i^j$为用载波波长表示的天线与接收机之间连接电缆所造成的载波相位延迟 (即线偏差) , b${}_i^j$是导航星星历和时钟误差产生的距离偏差, W${}_i^j$是测量噪声。考虑到编队卫星的体积、轨道高度, 该方程没有包括对流层延迟项和多径效应项。

主星与辅星的载波相位单差方程为

$Df{}_{mi}^j+l{\rm N}{}_{mi}^j=r{}_{mi}^j-\dot{r}{}_m^j\text{d}t{}_m+\dot{r}{}_i^j\text{d}t+C\text{d}t{}_m-C\text{d}t{}_i-{\rm I}{}^j{}_{mi}+W{}_{mi}^j(10)$

在载波相位单差方程中, 导航星钟差被消除, 在这里假设各编队卫星是完全相同的, 所以线偏差也被消除, 观测方程离散化后噪声的协方差矩阵为R。如果编队参考星与伴随星的距离为百米级, 可以认为它们到同一导航星的视线矢量相同, 则r${}_{mi}^j$可表示为

r${}_{mi}^j$=r${}_{mi}^j$-r${}_i^j$= (sj) Tρ (11)

式中, sj为编队参考星到第j颗导航星的视线矢量。实际应用中可以通过伪距定位与接收星历数据中包含的导航星坐标信息来确定。

对于钟差项, 可以用如下二阶马尔可夫过程进行模拟

式中, f为频漂项。

这样, 在编队卫星相对导航滤波系统设计中便可将式 (6) , 式 (7) 和式 (13) 作为系统状态方程, 式 (10) 作为系统量测方程, 进行UKF滤波器设计。

3 仿真及结果

本仿真以相对距离1 km左右对地观测双星编队为例, 进行相对导航滤波解算。编队卫星轨道参数, 如表1所示。

式 (6) 中过程噪声谱密度假设为$\sqrt{5}´10{}^{-11}\text{m}/\text{s}{}^{3/2}$, 载波单差量测方程噪声为0.05 mm, 钟差与频漂项噪声谱密度为:St=10-4 s2, Sf=10-11 s/s2, 在本仿真轨道高度条件下, 可忽略电离层影响。仿真时间5 min, 采样时间1 s。

GPS数据根据官方服务网站提供[6], 将上述编队卫星轨道参数与GPS参数输入STK软件进行仿真, 得到实时GPS量测数据, 在Matlab下进行滤波仿真, 仿真结果, 如图2 (a) 和图2 (b) 所示。

由仿真计算得到UKF算法均方根 (相对位置为:径向0.011 68 m, 切向0.014 53 m, 法向0.012 48 m;相对速度为:径向0.009 523 m/s, 切向0.006 789 m/s, 法向0.008 356 m/s) 。

以上数据明显看出, UKF算法能够提供高精度相对导航信息, 是可以在航天器编队飞行相对导航中广泛应用的非线性滤波方法。

4 结束语

CDGPS以其高精度而受到普遍重视, 在地面的测量中得到广泛的应用, 90年代以后在航天相对导航中逐渐得到应用, 其体积小、质量轻, 非常适合在编队小卫星的载荷受到限制的情况下代替现在普遍采用的导航设备实现编队的自主相对导航。

运用UKF滤波算法进行相对导航解算, 从结果可以得到UKF滤波算法达到很高的相对导航精度, 具有较高应用价值。

参考文献

[1]Randal W Beard, Jonathan Lawton, Fred Y Hadaegh.A Coordination Architecture for Spacecraft FormationControl[J].IEEE Transactions on Control SystemsTechnology, 2001, 9 (6) :777-790.

[2]张洪华, 林来兴.卫星编队飞行相对轨道的确定[J].宇航学报, 2002, 23 (6) :77-81.

[3]Julier S J.A New Method for the Nonlinear Transforma-tion of Means and Covariances in Filters and Estimators[J].IEEE Trans.on Automatic Control, 2000, 5 (3) :477-482.

[4]Xing G Q, Shabbir A P.Relative Attitude Kinematics&Dynamics and its Applications to Spacecraft Attitude StateCapture and Tracking in Large Angle Slewing Maneuvers[C].Space Control Conference, MITLincoln Laborato-ries, 1999 (4) :13-15.

[5]Hofmann W, Lichtenegger H, Collins J.GPS Theoryand Practice[M].New York:Springer, 1992.

UKF算法 第4篇

UKF容错滤波方法在自主导航中的应用研究

地磁场向量可描述成卫星的位置函数,以地磁场强度向量为观测量,利用三轴磁强计的测量信息即可实现近地卫星的自主导航.但当测量值存在野值时,滤波会出现偏差、收敛变慢甚至发散.将UKF滤波容错方法用于磁测自主导航,构造出一种基于残差正交性判别的UKF(Unsend Kalman Filter)容错滤波方法,来实现野值的`实时修正和故障诊断,使滤波器具有较强的鲁棒性.仿真结果表明该方法有效.

作 者：荣思远 常亚武 崔乃刚 RONG Si-yuan CHANG Ya-wu CUI Nai-gang 作者单位：哈尔滨工业大学航天工程系,哈尔滨,150001刊 名：宇航学报 ISTIC PKU英文刊名：JOURNAL OF ASTRONAUTICS年，卷(期)：27(4)分类号：V448.2关键词：自主导航 UKF 容错滤波 磁强计

UKF算法 第5篇

关键词：无迹卡尔曼滤波,采样策略,超球体平方根无迹卡尔曼滤波,强跟踪无迹卡尔曼滤波

1960年,美国数学家卡尔曼提出一种滤波方法,将其命名为卡尔曼滤波[1]。卡尔曼滤波的基本思想是将噪声融入系统的状态空间模型之中,对前一时刻采用估计的办法获得其估计值,对现在时刻采用其测量值,利用相关公式去预估计下一个状态的估计值。卡尔曼滤波是在维纳滤波的基础之上,利用线性最小二乘法求出系统状态估计的最优值[2]。因为是以线性最小二乘法为契机,当处理非线性系统时不能应用,但现实中几乎没有线性系统,很多非线性因素也不能忽略[3,4]。为了解决卡尔曼滤波在非线性系统中的使用障碍[5],经过研究,Bucy等学者利用泰勒公式将非线性系统展开成泰勒的一阶形式,使它得到近似的线性化,这样再按照线性卡尔曼滤波的方法处理问题。该方法被命名为扩展卡尔曼滤波[6](Extended Kalman Filtering,EKF)。但该方法在非线性较大时精度不够,易失去稳定性。此后,Julier S J等提出了无迹Kalman滤波(Unscented Kalman Filter,UKF),它是以无迹变换为基础,摒弃了扩展卡尔曼滤波将非线性系统线性化的做法,更好地处理了非线性化问题,同时提高了精度[7]。

1 标准UKF滤波①

标准UKF以标准无迹变换为基础,无迹变换的规则是在原有的状态分布的点集之中,采用一种规则选取一些采样点,保证所选点的均值和协方差与原状态点集均值和协方差相等,将采样点代入非线性系统之中,通过这些点集的变换,再求均值和协方差[8]。

给定一非线性离散系统:

定义Xk、Zk为系统的状态向量和量测向量矩阵;wk、vk为噪声观测值和量测值,wk~(0,Qk),vk~(0,Rk),将所述噪声规定为高斯白噪声,Q、R分别为其方差。

初始化估计值和状态协方差矩阵:

Sigma点计算:

通常将α的值设置为一个数值较小的正数,为矩阵n Pk-1均方根的第i列。

一步预测及其协方差矩阵时间更新:

其中,ωi为权值,

量测更新:

2 超球体采样平方根UKF(SR-UKF)

SR-UKF是在UKF算法的基础上采用一个状态协方差矩阵的方根矩阵形式[9,10,11],根据QR分解、Cholesky一阶更新和最小二乘法,在滤波过程中求出状态协方差矩阵的平方根,将所得值不断进行迭代,不再每一个时刻都对矩阵进行再分解,有效防止协方差矩阵非正定的发生[12]。在进行Sigma点选取时,采用超球体采样策略代替标准无迹变换可以有效地减少计算量。

选择0≤W0≤1。确定Sigma权值:

比例修正。采样点到中心x珋的距离随着维数的增加而越来越远,会导致采样的非局部效应问题,为防止这种问题,应采用比例采样修正[13]:

其中,α(0≤α≤1)为缩放因子,通过α可以控制采样点与均值的距离。

初始化向量序列:

当输入维数为j(j=2,…,n)时,迭代公式为:

其中,i是采样点顺序。

定义如下:

其中,β(β≥0)是非负加权项。

加入系统状态x的均值和协方差Px,生成的Sigma点为:

SR-UKF的算法如下:

a.初始化;

b.计算Sigma点;

c.时间更新;

d.量测更新方程。

步骤a采用的公式为:

其中规定矩阵下三角Cholesky分解运算简写成chol()。

步骤b采用的公式为:

步骤c采用的公式为:

其中qr(·)表示对矩阵进行奇异分解,cholupdata(S,U,±1)表示对下三角矩阵S进行Cholesky分解运算来更新,相当于计算矩阵下三角Cholesky分解运算

步骤d的公式为:

采用协方差阵的平方根形式Sx代替协方差矩阵Px参加递推运算,可以有效提高滤波算法的数值稳定性[14]。

3 强跟踪UKF

由于标准UKF算法缺乏对系统状态异常的自适应调整能力,导致滤波精度降低[15]。文献[15]提出一种改进的强跟踪UKF算法,该算法采用假设检验的方法对异常状态进行检测,当系统状态发生异常时,对预测协方差阵引入次优渐消因子[16,17]自适应地调整滤波增益,实现对系统真实状态的强跟踪。该算法中次优渐消因子的确定无需计算系统模型的雅克比矩阵。

强跟踪滤波器(Strong Tracking Filter,STF)的概念是根据新息向量的正交性原理提出的。这种滤波器与通常采用的滤波器相比,主要表现在它具有较强的关于模型参数失配的鲁棒性,同时对于噪声和初值统计特性的敏感性较低,当有突变状态产生时,STF具有极强的跟踪能力,同时在达到滤波定后,这种跟踪能力依然能够继续保持下去,与标准的UKF相比,STF的计算复杂性很低[18,19,20]。基于以上结论,可以将STF与标准的UKF结合起来,弥补标准UKF鲁棒性不足和对突变抵抗性差的问题。

将次优渐消因子λk引入预测协方差阵Pk|k-1以提高自适应能力:

计算新息序列:

记Vj=E[εk+jεkT],则:

令Vi=0,最终可得:

将式(28)代入式(24)可得:

其中V0为STF实际输出的基于新息的协方差矩阵,可根据下式计算:

其中,ρ为遗忘因子,0<ρ≤1,通常取为0.95。

给出一种确定系统异常的方法:定义一个统计量αk=εkT(HkPk/k-1HkT+Rk)-1εk,当统计量服从自由度为m的χ2分布时则有公式P(χ2>χ2a,m)=α成立,其中χ2a,m为阈值,通过阈值与统计量的比较可以推断系统的稳定性,如果统计量不大于阈值则系统稳定,如果大于阈值则系统存在异常。

强跟踪滤波UKF算法流程如下[21]:

a.按式(2)对式(1)进行初始化;

b.按式(3)进行Sigma点采集;

c.按式(4)、(5)进行一步预测;

d.如果有异常协方差矩阵采用式(24),如果没有则采用式(6);

e.按式(8)~(12)进行其余的计算。

4 结束语

UKF算法 第6篇

基于视频信息的编码方法研究的关键问题是如何准确地实现运动视频图像的分割及跟踪。Moscheni等人提出了利用空间、时间特性来描述目标运动特性,并实现对目标运动图像的分割与跟踪。该方法采用基于像素的区域搜索匹配方法实现目标的运动预测和属性判别,一旦目标运动状态突变或受到其他外界因素干扰时(如被其他目标遮挡或在视频传输出现丢帧现象时)便会导致算法失败。为了解决这一问题,许多学者将卡尔曼滤波(KF)和扩展卡尔曼滤波(EKF)算法引入到目标在线性和非线性状态下的图像分割与跟踪中[1,2,4]。然而,由于EKF仅采用非线性函数的Taylor展开一阶项,当系统出现强非线性时,或者目标距离跟踪区域较远时,EKF方法将有可能导致滤波发散。

针对机动目标跟踪中出现的非线性问题,改善滤波效果,Julier等人提出的基于Unscented变换(简称UT)的UKF目标跟踪算法应用于视频图像目标跟踪中,通过合理选择样本点来近似目标的状态分布,然后用样本点对非线性函数进行传递,使后验均值和协方差达到三阶(泰勒级数展开)精度[1]。在视频机动目标(如运动的昆虫、球类等)跟踪问题中,目标的先验知识非常有限,单一模型(如匀速模型(CV)、匀加速模型(CA)、转弯模型(CT))不可能完全准确描述机动目标所有运动状态的问题。交互多模算法(IMM)消除输入估计算法和变维滤波算法的机动检测滞后问题,对机动目标运动的全过程具有良好的跟踪性能[3]。但该算法在实现过程中滤波模式的确定和模式间转移概率的确定,需要一定的先验知识。

考虑到上述问题,本文提出一种UKF与简化IMM算法相结合的二维空间机动目标跟踪算法,该算法通过UKF进行滤波,通过门限检测目标是否发生机动,在未发生机动的情况下,用单模式进行跟踪,当检测到目标发生机动时,通过简化IMM算法进行模型交互及权值的自适应调整跟踪,从而克服了IMM算法中有限模式集合的限制以及非线性观测下转换坐标的线性化误差,减小了计算复杂度和对先验知识的要求,以提高系统的跟踪精度。

1 UKF算法的实现

1.1 目标状态方程与观测方程

设运动目标非线性系统的广义状态方程和观测方程为

式中:X(k)为目标状态向量;Z(k)为目标量测向量;w(k)为系统噪声,其方差Q(k);v(k)为量测噪声,其方差为R(k);u(k)为控制向量。f(⋅)为线性函数,h(⋅)为非线性函数。状态变量为,状态方差为。式中下标k对应第k个测量时刻。

1.2 Unscented变换与UKF滤波

Unscented变换是在非线性系统中计算统计量的一种新方法,它不像传统的EKF那样要对状态方程和观测方程进行线性化,而是在状态矢量Xk附近按照一定的规则选取有限的采样点。设Xkα是nx=n+q+p维随机变量,其均值为,方差为kPα,将采样点数据通过非线性系统传递,运用UT方法,状态矢量的统计特性就可得到近似。

由于UT变换没有对非线性函数进行线性化处理,即没有忽略高阶项,因而得到的后验分布均值和协方差估计比EKF方法得到的结果更加精确[3]。

首先根据系统均值和协方差Pkα,得到2nx+1个取样点Xαi,k及其相应的权值Wi,即

其中:是矩阵(nx+λ)Pkα平方根的第i行或第i列向量,系数,α和K为待选系数。然后计算每个采样点对应变换后的量,从而得到的均值和协方差估计。

UKF的时间方程更新,假设k-1时刻的状态向量和状态估计协方差分别为,则可以利用式(3)计算出相应的点和其对应的权值Wi。根据状态方程(1),可以得到一步预测:

UKF量测方程更新,量测和状态向量的交互协方差为

如果k时刻传感器提供的测量为Zk,则状态更新和状态更新协方差可表示为

式中:,当量测方程和状态方程都为线性方程时,时间更新及测量更新方程等都可得到简化,由UKF得到的滤波结果和标准的线性卡尔曼滤波得到的结果是相同的[5]。

2 简化的IMM算法

简化的IMM跟踪解决了IMM算法中有限模式集合和模式间的Markov状态转移概率参数的确定问题。在目标未发生机动时,用单模式进行跟踪,而一旦检测到目标发生机动时,简化的IMM算法根据新息v(k)的大小来判断所跟踪的目标是否发生了机动,并由此来调整跟踪滤波器的参数[7]。采用KF处理时,新息向量为

在非机动情况下,它是零均值高斯白噪声,其协方差矩阵为

式中:为一步状态预测的协方差矩阵。定义目标机动检测的判别函数为

由v(k)的统计特性可知,此处定义的机动检测判别函数D(k)是服从自由度为m的χ2分布,m为误差向量v(k)的维数。如果目标发生机动,新息将不再是零均值高斯白噪声,判别函数将变大。若D(k)>T,目标发生机动;若D(k)T,目标未出现机动,按原滤波参数跟踪目标[7]。

为了合理确定检测门限,要综合考虑两种概率:目标未发生机动而认为发生机动的虚警概率;目标发生机动而认为无机动的漏检概率。如果选择门限T较高时,则虚警概率变低,漏检概率变大。门限T的选择会影响机动检测的实时性,当选择门限T过大时,将产生较大的机动检测延时,影响跟踪的实时性。因此在确定门限T时,必须综合考虑虚警概率Pf和机动检测判别函数D(k)的变化情况。对于给定的虚警概率Pf,可得到门限T,而且它不是一个设计变量,只能通过改变窗口长度来减小机动检测平均延迟。

3 简化的IMM与UKF结合算法的实现

简化IMM与UKF结合算法的流程框图如图1所示,实现步骤如下:

step1:将k-1时刻目标模型的滤波值和误差协方差作为UKF滤波器的输入。根据式(13)确定判别函数D(k)。根据Pf和D(k)判别目标是否发生机动。

step2:如果未发生机动,则按原滤波参数单模式跟踪目标。如果发生机动,以和观测值Z0,k|k-1为输入,按照IMM算法对各模型进行交互得到k时刻的滤波值。

式中μij(k)是k时刻各模型的混合概率

step3:最后交互的输出

4 仿真实验与分析

4.1 仿真条件

以昆虫飞行视频图像为例,分别采用EKF与基于UKF的简化IMM目标跟踪算法对昆虫的飞行轨迹进行跟踪。摄像机静止放置距离目标约0.25 m,目标大小约1.5 cm,视频图像分辨率:640369,背景视场范围约:20 cm11.5 cm;截取视频片段的第1帧到第124帧,帧速:24帧/s,以两帧为一个测量时刻,进行62次测量跟踪;目标飞行轨迹:从花前飞行绕过花,直至落在花上,第1∼72帧目标为近似匀速直线运动(速度为5 cm/s)、第73∼124帧,目标绕花做转弯运动(转弯角速度ω=15°/s),其中第70帧到96帧目标被障碍物遮挡。目标飞行轨迹如图2所示。

设其X方向与Y方向的环境影响相同,水平加速度与垂直加速度的干扰均为独立高斯白噪声,观测误差也为独立高斯白噪声,初始状态X[]0=3[.2235.35.]T,状态矢量四个元素分别代表X(水平)、Y(垂直)方向位置(单位cm),X、Y方向速度(单位cm/s)。设定UKF算法中α=1,K=2;系统噪声协方差阵为Qk=diag1[0.11.0]1,观测噪声协方差阵为Rk=diag[3.50.1],设虚警概率Pf=0.05,窗口长度=5。IMM模型集采用“当前”统计模型、匀速模型、转弯模型。

4.2 仿真结果

图3、4分别为第12、116帧图像及采用基于UKF的简化IMM目标跟踪算法的跟踪结果。图5为两种算法的预测X方向误差分析,图6为两种算法的预测Y方向误差分析。

4.3 仿真结果分析

1)由图5、6可知:采用本算法的跟踪误差,在目标的非线性运动段(第1∼72帧、第97∼124帧,目标绕花飞行段)X方向最大误差为7像素,Y方向最大误差为6像素。在被遮挡段(第73∼96帧),X方向最大误差为6像素,Y方向最大误差为8像素;采用EKF算法的跟踪误差,在目标的非线性运动段,X、Y方向最大误差分别为10像素和12像素。在被遮挡段(第73∼96帧),X、Y方向最大误差分别为14像素和16像素。可以看出本滤波算法的跟踪性能明显由于采用EKF的跟踪算法。

2)虽然本算法跟踪精度明显优于EKF算法,但由于运算量的增大,采用UKF与简化IMM结合的算法跟踪实时性与EFK相比较差。

结束语

本文通过引入基于UKF的简化IMM目标跟踪算法,对常用的视频序列目标图像跟踪算法进行了改进。该算法有效的解决了目标运动状态在强非线性下的机动目标或小运动目标的实时估计与跟踪问题,但由于运算量增大,实时性有所降低。仿真实验表明,基于UKF的简化IMM目标跟踪算法的跟踪性能优于EKF滤波算法。经过进一步的验证表明,该算法不仅适用于非线性条件下的单一目标的跟踪问题,同样也适用于对多目标的跟踪研究。

参考文献

[1]Julier S J,Uhlmann J K.Reduced sigma point filters for the propagation of means and covariances through nonlinear trans formations[C]∥Proceedings of the American Control Conference,Anchorage,May8～10,2002,Alaska,2002:887-892.

[2]张江山,朱光喜.一种基于Kalman滤波的视频对象跟踪方法[J].中国图象图形学报,2002,7(6):605-609.ZHANG Jiang-shan,ZHU Guang-xi.Kalman Filter for Video Object Segmentation and Tracking[J].Journal of Image and Graphics,2002,7(6):605-609.

[3]Julier S J,Uhlmann J K,Durrant-Whyte H F.A new approach for filtering nonlinear systems[C]∥Proceedings of the American Control Conference,Seattle,July21-23,1995.Washington1995,3:1628-1632.

[4]魏娟丽,翟社平,王万诚.视频序列中人体运动目标的检测与跟踪研究[J].计算机应用与软件,2006,23(4):139-142.WEI Juan-li,QU She-ping,WANG Wan-cheng.Research for Detecting and Tracking of Human Motion Target in Video Based Sequences[J].Computer Applications and Software,2006,23(4):139-142.

[5]张卫明,张继惟,范子杰,等.UKF方法在惯性导航系统初始对准中的应用研究[J].系统工程与电子技术,2007,29(4):589-592.ZHANG Wei-ming,ZHANG Ji-wei,FAN Zi-jie,et al.Research of unscented Kalman filter in initial alignment of inertial navigation systems[J].Systems Engineering and Electronics,2007,29(4):589-592.

[6]张苗辉,杨一平,刘先省.基于UKF的机动目标跟踪算法[J].火力与指挥控制,2007,32(8):37-39.ZHANG Miao-hui,YANG Yi-ping,LIU Xian-sheng.The Algorithm of Maneuver ing Target Tracking based on UKF[J].Fire Control and Command Control,2007,32(8):37-39.