卡尔曼滤波算法

卡尔曼滤波算法（精选十篇）

卡尔曼滤波算法 篇1

时滞是控制系统的常见特性，是指从一个系统到另一个系统或者系统内部各组件之间信息传输时产生的延迟[1]，它在各类实际工业系统和应用中普遍存在。通常情况下，根据时滞产生的来源，时滞系统可分为状态方程中含有时滞的时滞系统和观测结果中含有时滞的时滞系统[2]。因此时滞系统的滤波问题也分为两类，即时滞出现在系统的状态方程中的滤波问题和时滞出现在观测方程中的滤波问题。

近几十年里，学者们主要采用增广状态方法、偏微分Riccati方程和线性矩阵不等式法解决状态时滞系统的滤波问题[3,4]，但这些方法都存在一定程度的缺陷，比如增广状态方法使计算量大幅增加，偏微分Riccati方程的求解难度大且很难对所得到的控制器和滤波器进行性能分析，线性矩阵不等式法的凸优化问题的构造难度大且无法从理论上彻底解决滤波问题。而观测时滞系统的滤波问题的研究则主要是信息重组方法，但此方法只适用于线性观测时滞系统中同时含有即时观测和滞后观测的情况下的滤波问题[5,6]。

在观测时滞系统中，系统模型方程中含有观测时滞是导致传统卡尔曼滤波方法无法直接应用的根本原因。为此，笔者借鉴系统状态增广方法，研究了一种将观测时滞方程转换为无时滞观测方程的方法，针对转换后的不含观测时滞的系统方程模型，给出观测时滞系统的卡尔曼滤波算法的详细推导过程和算法步骤，并将推导出的算法应用于实际非线性测量光电跟踪系统中进行预测滤波对比实验。

1 观测时滞系统的状态估计

通常情况下，随机非线性观测时滞离散系统的方程可表示为:

式中f(·)———系统的非线性状态函数;

h(·)———系统的非线性测量函数;

vk-r———观测噪声;

wk-1———过程噪声;

xk———系统的n维状态向量;

zk———系统的m维观测量;

下标k、r———当前时刻和滞后时间。

系统过程噪声wk∈Rn和观测噪声vk∈Rm均为高斯白噪声，且互不相关，其统计特性为:

式中Qk———对称非负定矩阵;

Rk———对称正定矩阵。

根据状态估计原理，观测时滞系统的预测滤波问题就是用含有噪声的在离散时刻t1,t2，…，tk-r的测量数据序列Z1:k-r={z1,z2，…，zk-r}对状态向量Xk在k时刻进行估计。因此，如果针对式(1)、(2)描述的观测时滞系统按照传统的状态估计方法，只能得到k-r时刻的状态估计值，而无法得到k时刻的状态估计值。导致传统状态估计方法无法直接应用的根本原因在于观测时滞系统模型的测量方程与传统状态估计方法中的测量方程的形式有所不同，观测时滞系统的测量方程具有一定周期或者时间的时滞，观测时滞系统的测量序列为k-r时刻及其之前时刻的测量值。

由此可以看出，若能探索一种将观测时滞方程表示为无时滞的观测方程，并且符合传统预测滤波算法的表达形式的方法，则可解决传统预测滤波算法无法在观测时滞系统中直接应用的问题。

2 基于系统状态增广的无时滞系统状态估计

2.1 系统状态增广的无时滞系统模型

对于式(1)、(2)描述的时滞系统，重新定义状态向量Xk=[xkTxkT-1…xkT-r]T，并在该时滞系统方程中采用Xk为新的状态变量，则原观测时滞离散系统的方程变为:

其对应的矩阵方程可表示为:

其中，Xk和Xk-1分别是k时刻和k-1时刻的增广状态向量;Wk-1=[wk-10…0]T和Vk=vk-r分别为引入增广状态之后的过程噪声和观测噪声。从式(6)、(7)描述的观测时滞系统模型中可以看出，式(6)仍然符合传统预测滤波算法的系统方程表示形式，只是系统状态维数增大。而式(7)依然不符合传统预测滤波算法的观测方程表示形式。因此，系统状态增广方法并不能解决传统预测滤波在观测时滞系统中的直接应用问题。

解决上述问题的关键仍然是寻求一种能将观测方程转换为传统预测滤波算法可直接应用的形式的方法，即k时刻的观测量zk与k时刻的状态量Xk之间的关系表达式。式(7)虽然描述的是k-r时刻观测量zk-r与k时刻的状态量Xk之间的关系，但其实质仍然是k-r时刻观测量zk-r与k-r时刻状态量Xk-r之间的关系。因此，借鉴式(7)的本质意义与方程式的表达形式，重新定义Zk=[0…0 I][zkzk-1…zk-r]T，则式(6)、(7)描述的观测时滞系统可表示为:

式中f*(·)、h*(·)———与式(6)、(7)相同的非线性映射关系;

Wk、Vk———引入增广状态之后的过程噪声和观测噪声，且互不相关;

Xk———k时刻的增广状态;

Zk———k时刻的观测量。

可见，式(8)、(9)描述的系统方程中观测时滞已经不存在，为无时滞的系统模型方程，是符合传统预测滤波算法直接应用的表示形式。

2.2 系统状态增广的无时滞系统的卡尔曼滤波算法

2.2.1 增广状态扩展卡尔曼滤波算法

扩展卡尔曼滤波(Extended Kalman Filtering,EKF)算法是一种能够解决非线性系统滤波问题的有效方法。因式(8)、(9)描述的系统仍然为非线性系统，因此这里将增广状态法引入EKF算法得到针对时滞系统的增广状态EKF算法。与EKF算法类似，使用一阶线性化方法，将非线性系统方程展开泰勒级数得到线性的系统方程模型:

其中，Xk和Xk-1分别是k时刻和k-1时刻的增广状态向量;Φk|k-1为系统的状态转移矩阵;Hk为观测矩阵;Vk=vk-r和Wk-1=[wk-10…0]T分别为引入增广状态之后的测量噪声和过程噪声，其统计特性为:

其中，Qk*是系统过程噪声Wk的非负定方差矩阵;Rk*是系统观测噪声Vk的正定方差矩阵，与原系统方程的噪声方差矩阵的关系为:

式(10)、(11)中:

根据卡尔曼滤波原理，系统状态增广的无时滞系统k时刻的状态估计分为状态更新和量测更新两步。

状态更新:

量测更新:

综上所述，对于式(1)、(2)描述的观测时滞系统，在采用基于增广状态的系统模型变换法将原系统模型变换为无时滞的系统模型方程后，可根据前r个时刻的状态值x0，…，xr和方差Pr，递推计算出k时刻的增广状态估计值，进而得到状态估计^xk的表达式为:

2.2.2 增广状态无损卡尔曼滤波算法

由于EKF算法的核心思想是对非线性模型进行一阶线性化，略去了二阶以上的项，这就导致了EKF算法在高阶系统的应用中滤波精度较低，增广状态EKF算法依然继承了这一特点。为了克服此缺点，将增广状态法引入无损卡尔曼滤波(Unscented Kalman Filter,UKF)算法，该算法的核心思想是使用无损变换代替EKF算法的一阶线性化，其精度远高于EKF算法。

将增广状态法应用于式(8)、(9)描述的非线性系统，初始状态统计特性为:

选择无损变换中的Sigma采样策略，以对称采样为例，首先取L=2n(n为系统状态变量的维数)，通过对称采样可得到2n+1个Sigma点，对称采样下的Sigma点和每个点所对应的权系数为:

对应于ξi(i=0,1，…，2n)的权值为:

其中，κ是特定的比例系数，用来调节Sigm点和距离，且对高阶矩产生的误差有所影响表示(n+κ)Px的平方根矩阵的第i行或第i列。

按照所选择的Sigma采样策略，由和Pk-1计算Sigma点ξi,k-1(i=0,1，…，L)，通过非线性状态函数fk*-1(·)+qk*-1计算γi,k|k-1，由γi,k|k-1可得状态预测^Xk|k-1和误差协方差阵Pk|k-1:

同理，利用和Pk|k-1按照所选择的采样策略计算Sigma点ξi,k-1，通过非线性量测函数hk*-1(·)+rk*-1计算χi,k|k-1，由χi,k|k-1可得输出预测、自协方差阵P珘Zk和互协方差阵P珘Xk珘Zk，即:

在获得新的量测Zk后，进行滤波量测更新:

与增广状态EKF算法类似，根据前r个时刻的状态值x0，…，xr和方差Pr，递推计算出k时刻的增广状态估计值^Xk，然后根据式(17)得到k时刻原系统的系统状态xk的估计值^xk。

3 非线性光电跟踪观测时滞系统仿真实验

为实现非线性延迟光电跟踪系统目标运动参数的增广状态EKF算法和UKF算法实验，做以下4点设计。

为重点表现非线性测量，实验选取CV和CA模型作为目标运动模型。该系统的状态变量为跟踪目标的运动参数(目标位置、目标运动速度和目标运动加速度)，假设该系统的噪声为互不相关的高斯白噪声。

将极坐标系下的测量模型转换为直角坐标系下的非线性测量模型，并以此为光电跟踪目标测量模型，离散化光电跟踪测量时滞模型，有其中，目标测量值Z由距离r、方位角a和俯仰角e组成;假定vr、va、ve为互不相关的高斯白噪声;方差分别为σ2r、σ2a、σ2e，测量噪声方差Rk=diag{σ2r，σ2a，σ2e}。

测量数据取某靶场对实际空间随机运动目标进行跟踪的实测数据，其采样周期T=0.00625s，测量误差σr=5m、σa=σe=3″。

采取对比仿真实验的方法，对增广状态EKF算法和增广状态UKF算法的性能进行对比。CV和CA模型下的两种滤波算法的均方根误差和均值误差统计数据见表1、2,CV和CA模型下的两种滤波算法的预测误差曲线如图1、2所示。

通过分析表1、2中的数据可以发现，增广状态UKF算法各方面的性能均优于增广状态EKF算法，该算法的平均误差和均方根误差都非常小，滤波精度较高。实验结果证明，针对测量时滞系统预测滤波问题的增广状态EKF算法和增广状态UKF算法都是有效可行的，其中增广状态UKF算法的滤波性能更强。

从图1、2可以看出，在两种目标运动模型下，增广状态EKF算法和增广状态UKF算法的预测误差曲线均为收敛，其中增广状态EKF算法调节时间较长，滤波初期的误差较大，尤其是距离误差初期的偏离最为严重。而增广状态UKF算法在保证误差曲线收敛的前提下，相比于增广状态EKF算法，其调节时间更短，并且极大地降低了距离误差，俯仰角误差和方位角误差也得到了有效改善。

4 结束语

在实际应用中，测量结果中带有时滞的情况是很普遍的，此类系统比无时滞的系统更加复杂，因此传统的卡尔曼滤波算法无法直接应用于此类系统的估计问题中。为此，基于增广状态法，给出了适合非线性测量时滞系统的增广状态卡尔曼滤波算法结构及其估计问题的计算步骤，并将该算法应用于运动目标光电跟踪实验，验证了在具有非线性测量时滞的情况下增广状态算法的可行性和性能效果。

摘要：由于观测时滞系统的观测方程不符合卡尔曼滤波算法的实现形式,导致传统卡尔曼滤波算法无法直接应用于观测时滞离散系统。为此,在借鉴系统状态增广方法的基础上,设计了一种将观测方程转换为无时滞观测方程的方法。同时根据卡尔曼滤波算法原理,针对变换后的无时滞系统模型方程,给出观测时滞卡尔曼滤波算法的步骤,并将此方法应用到实际非线性观测光电跟踪系统中,进行算法的性能对比。仿真实验结果证明:将时滞观测方程转换为无时滞观测方程的方法是可行的,可大幅度提高算法精度。

关键词：卡尔曼滤波算法,非线性观测光电跟踪系统,非线性滤波,增广状态,状态估计

参考文献

[1]蔡云泽.时滞不确定性系统的鲁棒滤波研究[D].上海:上海交通大学,2003.

[2]王好谦.具有观测时滞的广义系统的最优状态估计[D].哈尔滨:哈尔滨工业大学,2005.

[3]Fridnan E,Shaked U.New Bounded Real Lemma Representations for Time-Delay Systems and Their Applications[J].IEEE Transactions on Automatic Control,2001,46(12):1973~1979.

[4]Ren J S.LMI-Based Fault Detection Filter Design for a Class of Neutral System with Time Delay in States[C].The Sixth World Congress on Intelligent Control and Automation.Dalian:IEEE,2006:5581~5585.

[5]张志钢,张承慧,崔鹏,等.有观测时滞线性系统的白噪声最优估计[J].山东大学学报(理学版),2009,44(6):63~68.

卡尔曼滤波算法 篇2

在系统机动性不强的情况下,传统的平台内阻尼算法将系统本身的速度信息通过阻尼网络加到系统中,达到提高姿态角精度的目的.将这种平台内阻尼的思想引入到捷联惯性航姿系统中,在系统加速度较小的情况下,利用加速度计的输出估计系统姿态角,通过卡尔曼滤波的形式补偿系统姿态误差.由于加速度的.大小直接影响滤波器精度,本文设计了模糊自适应卡尔曼滤波算法,根据三轴加速度计的输出调整内阻尼量测误差方差阵,从而避免了滤波器的发散.仿真和实验验证,内阻尼算法可明显抑制舒勒周期振荡和傅科周期振荡,避免了系统姿态漂移,有效提高了捷联惯性航姿系统的精度.

作 者：杜亚玲 刘建业 刘瑞华 祝燕华 DU Ya-ling LIU Jian-ye LIU Rui-hua ZHU Yan-hua 作者单位：杜亚玲,DU Ya-ling(宇航智能控制技术国防科技重点实验室,北京,100854)

刘建业,祝燕华,LIU Jian-ye,ZHU Yan-hua(南京航空航天大学自动化学院导航研究中心,南京,210016)

刘瑞华,LIU Rui-hua(中国民航学院天津市智能信号与图像处理重点实验室,天津,300300)

卡尔曼滤波算法 篇3

摘要：针对动态目标的GPS定位精度不高和实时性较低的问题，通过改进卡尔曼滤波的定位算法，有效消除GPS动态数据的随机误差，给出了仿真得出的运动轨迹对比图和误差曲线对比图.该算法将速度观测量加入到常规的卡尔曼方程中，得出带约束项的改进型卡尔曼方程.通过实验结果可以看出该算法可以有效提高动态目标的GPs定位精度和实时性，具有重要的理论和实际意义.

关键词：动态目标；GPS定位；卡尔曼方程；约束项

Dol：10.15938/j.jhust.2016.04.00l

中图分类号：TN966

文献标志码：A

文章编号：1007—2683（2016）04—0001—06

0引言

全球定位系统（GPS）自1973年首次由美国国防部部署开始，凭借自身的高效益、高精度、自动化、全球性、全天候等显著特点，被广泛应用到大地测量、工程测量，水利、电力、交通、资源勘探和航海等领域中。

由于GPs的现代化进程飞速发展，其应用的范围越来越广泛，对其精度和速度也有了更高的要求.通过文献查阅可知，GPS动态定位一般可以分为伪距动态定位和载波相位动态定位.伪距定位平面坐标的精度可以最小可以达到0.48 m，载波相位定位平面坐标精度约为O.004m，但目前在GPS定位算法方面的研究还很不足，在滤波的过程中所用到的定位方程运算复杂，计算量很大，定位的精度低，速度慢，不适用于对动态目标的定位，本文对定位算法进行改进，通过分析GPS接收机的定位结果，设一个误差总和，其包含各种误差因素，由于定位的精度会受到速度观测量的影响，现将速度视为约束条件的卡尔曼方程与将速度视为观测量的常规卡尔曼方程联立.可以提高定位的精度和实时性，简化方程，减少计算量，对于GPS定位算法的发展而言，具有重要的理论和实际意义。

1.GPS定位误差分析

GPS为导航定位、测量和测量学开辟了一个崭新的时代.但由于GPS定位中含多种误差，利用传统的方法很难将其影响去除，使得GPS在某些场合的应用还受到限制，GPS定位的主要的误差源包括：①卫星测量误差.可分为：卫星时钟误差、星历误差、电离层的附加延迟误差、对流层的附加延时误差，②卫星的几何位置造成的定位误差，在运动目标定位中误差主要源于如下几种：多路径效应、信号遮挡、信号丢失、GPS接收机动态测量范围的局限性而引起的定位误差及首次定位的时延误差等。

影响GPS定位结果的误差因素也同时影响其测速情况，不过，接收机误差因素的影响表现显著，其他因素影响稍弱.接收机误差因素中接收设备的锁相跟踪环路掺人噪声，产生虚假的多普勒频移是其主要影响GPS测速的因素。

2.观测速度量的GPS定位算法

将误差因素均看成零均值的高斯白噪声，而实际情况中，很多信号显示为非白噪声分布.为了能提高GPS对于动态目标的定位精度和稳定性，对其信号的处理方式选用了卡尔曼滤波（kalman filter）算法.其原理是根据信号的前次估计值和当前观测值，通过利用线性无偏最小方差估计准则来推算得出当前的信号值，无需考虑以往观察值.还需已知状态方程、量测方程、白噪声激励的统计特性、量测误差的统计特性，其所用的信息均是属于时域内的.卡尔曼滤波适用于多维，其适用范围十分广泛，其特点有以下4种：

1）可以通过计算处理随机信号.

2）可以区分出所用被处理信号是否有用和干扰。

3）利用系统的白噪声激励和量测噪声的统计特性进行估算，不将其滤除。

4）算法属于递推类型，且在时域内使用状态空间法，适用多维随机过程。

卡尔曼滤波只利用信号的前次估计值和当前观测值，就可以对其进行线性无偏最小方差.

由于动态目标在运动过程具有复杂性，根据常规滤波方程处理可以发现其过程复杂、计算量大、实时性低等缺点.在实际使用的情况下，通过在GPS接收机上接收到的定位坐标进行预处理，并将所有的得到的误差加和计算.达到简化方程；减少数据处理量；减少得到的运算结果延迟性和准确性的目的。

2.2基于速度量的算法改进

2.2.1状态方程

在定位的实际应用中主要考虑平面的位置情

2.2.2改进后量测方程

比较GPS定位的速度数据和位置数据可得，其原理不相同，准确性则不同，速度数据的准确性高于位置数据的准确性，GPS定位过程中会输出的动态目标位置信息和速度信息，其中速度信息可分为速度的大小和方向，速度观测量影响GPS的定位精度，在观测方程中加入速度观测量可以提高定位精度，提高状态的可观测度。

将约束方程代人卡尔曼模型中，使得原本的卡尔曼方程的状态参数不满足新的条件方程。需要对其进行相应的修改，实现准确的滤波推导.根据采用最小方差估计原则，推导出新的带有速度约束条件的卡尔曼递推方程，并且保留原有发状态参数.

建立拉格朗日方程：解之得：最后得到带速度约束条件的卡尔曼算法递推公式：

它与常规的卡尔曼递推算法进行比较，改进后卡尔曼算法在预测值后加了一个和约束条件有关的修正项，没有对其他步骤进行改变.修正项是表示为最大化符合速度约束条件的要求，不间断的对载体运动的轨迹进行修正。

3.改进后算法仿真与对比

我们设定某一目标的起始位置在坐标轴的（0，0）上，本目标在x和Y轴上的初始加速度均为O.1m/s，设定本目标在坐标系内的运动为非规律运动，并且x和Y轴上分别给加速度增加了噪声影响，并且可以得到运动目标的运动轨迹.通过设定两部对目标进行实时测量的观察机以用来得知目标的相对距离.假设运动目标的状态向量、观测向量分别是：

随后我们使用标准型卡尔曼方程对运动目标的轨迹进行滤波处理，其中它的系统噪声和量测噪声都是非相关独立高斯白噪声，其中设定时间T为1秒，在第3个历元介人滤波.全部过程耗费300秒，可以得到运动目标的轨迹如图1所示。

通过卡尔曼算法得到的值同真实轨迹值相减，求得滤波的误差曲线.如图2所示为其x轴上的误差。

通过以上的方式，替换量测方程为线性方程，使用改进后的卡尔曼算法再次进行仿真，得到的运动目标轨迹如图3.

通过图1和图3对比常规卡尔曼算法得到的轨迹和改进后卡尔曼算法得到的轨迹，可以看出后者更接近真实轨迹.通过图2和图4对比常规卡尔曼算法得到的误差曲线和改进后卡尔曼算法得到的误差曲线，可以看出后者误差明显减小.其原因是在相同条件下，改进后卡尔曼算法是将速度观测量代入状态变量中，并且作为速度约束条件代入卡尔曼方程，在系统方程不变的情况下，改进后的量测方程转换为：

而后利用Z₁和Z₂可以得到常规卡尔曼方程以及带约束项的卡尔曼方程，而后通过之前的运动轨迹模型在矩阵实验室中进行仿真，如图5和图6所示。

通过对比图可以直观得到带有速度约束项的改进后卡尔曼方程是很有效的降低了误差度.其误差值给出了比较有代表性的，如表1所示.

其原因为观测速度量的加入约束了状态量的变化，使物理意义与数学关系结合的更加直接，其改进后结果对预测值进一步进行了修正.这么做的优点是提高了坐标精度，得到的图像也证明了此点.综上所述：带有速度限制算法的卡尔曼量测方程能提高预测精度。

4.结论

卡尔曼滤波算法 篇4

计算机视觉领域中,运动目标跟踪是关键问题之一,其在视频编码、视频监控、军事、交通等领域有着重要而广泛的应用[1]。目标跟踪,就是在一段图像序列的每幅图像中找到所感兴趣的运动目标所处的位置,从而达到跟踪的目的。跟踪算法的实时性取决于匹配搜索策略和滤波预测算法,跟踪算法的精度和鲁棒性很大程度上取决于对运动目标的表达和相似性度量的定义。但迄今为止,运动目标跟踪算法的鲁棒性、准确性和实时性的统一仍是尚未解决好和正在努力追求的目标。

目前常用的跟踪算法如均值漂移算法(Meanshift)在目标跟踪过程中没有利用目标在空间中的运动方向和运动速度信息,当周围环境存在干扰 (如光线、遮挡)或运动速度过快时容易丢失目标。连续自适应均值漂移算法(Camshift)在均值漂移算法的基础上虽结合目标色彩信息进行了改进,但需在开始前由人工指定跟踪目标[2]。粒子滤波算法在非线性、非高斯系统取得了较好的跟踪效果但计算量相对较大,实时性有待提高。而Kalman滤波算法可以方便地预测目标的位置和速度, 具有稳定、计算量小的特点,但是单纯使用Kalman滤波跟踪准确性不高。文献[3]中提出了一种基于遗传算法和卡尔曼滤波的运动目标跟踪算法,取得较好的效果。粒子群算法是一种高效的搜索算法,具有实现方便、收敛速度快、参数设置少的优点。与遗传算法比较,所有的粒子在大多数情况下可能更快的收敛于最优值[4]。

综合以上考虑,本文提出了粒子群算法和卡尔曼相结合的新的运动目标跟踪算法。利用卡尔曼滤波预测目标中心在下一帧图像中的位置,并以该位置为中心建立目标搜索区域。然后以目标的灰度统计特征对目标模板和候选区域进行匹配,确保跟踪准确性。利用粒子群算法在搜索区域找到和目标模板最相似的区域,粒子所在的位置即为最优中心位置,并以该位置作为卡尔曼滤波的观测值,进行下一帧跟踪。

1 算法原理依据

1.1 卡尔曼滤波

卡尔曼滤波是由一系列递推数学公式描述。其信号模型是由离散的状态方程和观测方程组成的[5]。

设离散时间过程状态变量x∈Rn,状态方程可描述为:

$\mathit{x}{}_k=\mathit{A}x{}_{k-1}+\mathit{B}u{}_{k-1}+\mathit{w}{}_{k-1}(1)$

式中:xk是在k时刻系统的n维状态矢量;A是系统从k-1时刻到k时刻的n×n一步状态转移矩阵; n×l阶矩阵B代表可选的控制输入u∈Rl的增益;wk-1为k-1时刻时的过程激励噪声。

设观测变量z∈Rm,得到观测方程:

$\mathit{z}{}_k=\mathit{{\rm H}}\mathit{x}{}_k+\mathit{v}{}_k(2)$

式中:zk是k时刻的m维观测信号矢量;H是k时刻的m×n阶观测矩阵;vk是观测噪声。

离散卡尔曼滤波器的工作原理,算法的具体实现是由时间更新方程和测量更新方程来实现。具体形式如下[5]:

离散卡尔曼滤波器的时间更新方程:

$\begin{array}{l}\widehat{\mathit{x}}{}_{k|k-1}=\mathit{A}\widehat{\mathit{x}}{}_{k-1}+\mathit{B}u{}_{k-1}(3)\\ \mathit{{\rm P}}{}_{k|k-1}=\mathit{A}\mathit{{\rm P}}{}_{k-1}\mathit{A}{}^{\text{Τ}}+\mathit{Q}(4)\end{array}$

式中:$\widehat{\mathit{x}}$k|k-1为状态一步预测矢量,即向前推算状态变量;$\widehat{\mathit{x}}$k-1为k-1时刻的状态滤波值;Pk|k-1为一步预测均方误差阵,即向前推算误差协方差;Pk-1为k-1时刻的滤波均方误差阵,由上可知时间更新方程主要完成预测。

离散卡尔曼滤波器的测量更新方程为:

$\begin{array}{l}\mathit{{\rm K}}{}_k=\mathit{{\rm P}}{}_{k|k-1}\mathit{{\rm H}}{}^{\text{Τ}}(\mathit{{\rm H}}\mathit{{\rm P}}{}_{k|k-1}\mathit{{\rm H}}{}^{\text{Τ}}+\mathit{R}){}^{-1}(5)\\ \widehat{\mathit{x}}{}_k=\widehat{\mathit{x}}{}_{k|k-1}+\mathit{{\rm K}}{}_k(\mathit{z}{}_k-\mathit{{\rm H}}\widehat{\mathit{x}}{}_{k|k-1})(6)\\ \mathit{{\rm P}}{}_k=(\mathit{{\rm I}}-\mathit{{\rm K}}{}_k\mathit{{\rm H}})\mathit{{\rm P}}{}_{k|k-1}(7)\end{array}$

式中:Kk为滤波增益矩阵;zk为观测值,它联合增益矩阵Kk来修正一步预测值$\widehat{\mathit{x}}$k|k-1,即由观测变量更新估计;Pk用来更新误差协方差。

1.2 粒子群算法

粒子群算法最早是在1995年由Eberhart和Kennedy共同提出的,其基本思想源于对鸟类群体行为的研究[6]。假设一个场景:一群鸟在某一区域搜索食物,但是它们都不知道食物的具体位置,而知道当前的位置离食物到底有多远,这样,找到食物最有效的方法就是搜索当前离食物最近的鸟的周围区域。PSO就是基于这种思想来解决优化问题的。在PSO中,鸟群中的每一只鸟可以看作是每一个优化问题的解,也就是“粒子”,所有的粒子都有一个被优化的适应值,每一个粒子还有一个速度v来确定它们飞行的方向和距离。然后,粒子们像鸟群在离食物最近的鸟周围搜索一样,追随当前最优粒子(离食物最近的鸟)在解空间中搜索[7]。PSO在每次的搜索过程中,粒子通过跟踪两个极值来更新自己:一个为粒子自身所找到的最优解,称为个体极值Pbest,另一个为整个种群当前找到的最优解,称为全局极值Gbest。例如,在一个D维的目标搜索空间中,每个粒子看成是空间内的一个点,设群体由m个粒子构成。

设zi=(zi1,zi2,…,ziD)为第i个粒子(i=1,2,…,m)的D维位置根据设定的矢量,根据设定的适应值函数计算zi当前的适应值,可衡量粒子位置的优劣;vi=(vi1,vi2,…,vid,…,viD)为粒子i的飞行速度,即粒子移动的距离;pb=(pb1,pb2,…,pbd,…,pbD)为粒子迄今为止搜索到的最优位置;pg=(pg1,pg2,…,pgd,…,pgD)为整个粒子群迄今为止搜索到的最优位置。

在每次迭代中,粒子根据以下式子更新速度和位置:

$\begin{array}{l}v{}_{id}^{k+1}=wv{}_{id}^k+c{}_{1}r{}_1(p{}_{\text{b}d}-z{}_{id}^k)+c{}_{2}r{}_2(p{}_{\text{g}d}-z{}_{id}^k)(8)\\ z{}_{id}^{k+1}=z{}_{id}^k+v{}_{id}^{k+1}(9)\end{array}$

式中:i=1,2,…,m;d=1,2,…,D;k为迭代次数,r1和r2为[0,1]之间的随机数;c1和c2为学习因子;由于粒子群算法中没有实际的机制来控制粒子的速度,所以有必要对速度的最大值进行限制,设其为vmax,位置zi的取值范围为zmin～zmax;w为惯性权重,它起着权衡局部最优能力和全局最优能力的作用。大量实验表明,不把惯性权重设为定值,而是设为一个随时间线性减少的函数效果更好,惯性权重的函数形式通常为:

$w=w{}_{\max }-\frac{w{}_{\max }-w{}_{\min }}{\text{i}\text{t}\text{e}\text{r}{}_{\max }}\times k(10)$

式中:wmax为初始权重;wmin为最终权重;k为当前迭代次数;itermax为最大迭代次数[4]。

2 算法的具体实现过程

2.1 目标灰度特征和粒子群适应度函数

目标灰度特征对目标的形变、旋转等不敏感,具有良好的稳定性。在目标图像和候选图像匹配过程中,采用图像的灰度特征进行匹配[8]。

设图像灰度级为L(0～L-1),一般L为256。为加快运算速度,提高跟踪实时性,把灰度级映射为m级,如m=8,即把灰度级划分为8个级别。则目标模板直方图特征可表示为: q={qu,u=0,1,2,…,m-1}。$q{}_u=\frac{1}{n}{\displaystyle \sum _{i=1}^n\delta }[b(x{}_i)-u],u=0,1,2,\cdots ,m-1$。其中b(xi)是位于点xi灰度值特征映射函数,设该点灰度值为g,则$b(x{}_i)=\left[g\frac{m}{L}\right]$;[x]表示取不大于x的整数;δ(·)为Kronecker delta函数;n是目标模板的像素总数。

相应的,假设中心点在y点,则候选目标的灰度特征可以表示为p(y)={pu(y),u=0,1,2,…,m-1}。其中$p{}_u(y)=\frac{1}{n}{\displaystyle \sum _{i=1}^n\delta }[b(\widehat{\mathit{x}}{}_i)-u],u=0,1,2,\cdots ,m-1‚\widehat{\mathit{x}}{}_i$是候选目标区域的像素点[3]。采用Bhattacharyya系数[9]来度量目标模板与候选目标区域的相似性,其定义为:

$\rho (y)=\rho [p(y)q]={\displaystyle \sum _{u=0}^{m-1}\sqrt{p{}_u(y)q{}_u}}(11)$

于是,跟踪过程中的目标定位问题转化成了求使上式取得极大值的候选目标区域的问题。当ρ(y)越接近1,表示目标与候选区域越相似;当ρ(y)=1时,表示目标和候选区域完全相似。式(11)是作为粒子群算法采用的适应度函数。

2.2 粒子群与卡尔曼滤波的结合

利用Kalman滤波可以预测跟踪目标在下一帧图像的位置,其中心点设为(x0,y0),以该点为中心,取宽度为w,高度为h的区域为搜索区域(如图1所示),也就是要在该区域找到和目标模板最相似的候选区域中心点。为了使用粒子群算法和卡尔曼滤波相结合,在该区域中心点(x0,y0)的周围撒一些粒子,如图1所示,再以每个粒子为中心,分别建立宽度为w,高度为h的搜索区域,这样就形成了m(粒子群规模)个候选区域,而以上知道粒子群的适应度函数为目标模板和候选区域的灰度特征相似性,这样就可以应用粒子群算法求一个最优解,即与目标模板最相似的运动目标中心(pg1,pg2),将这个最优解赋值给式(6)中的zk,再与$\mathit{{\rm H}}\widehat{\mathit{x}}{}_{k|k-1}$做差,得到的差值结合增益Kk来修正卡尔曼的状态一步预测值$\widehat{\mathit{x}}$k|k-1,得到当前时刻的状态滤波值$\widehat{\mathit{x}}$k,得到的$\widehat{\mathit{x}}$k再代入式(3)进行下一步的预测。具体的程序流程图如图2所示。

3 实验结果与分析

该算法实现都在Matlab 7.0.1环境下,采用CAVIAR数据集中WalkByShop1front和OneLeaveShopReenter2cor两段视频测试序列为实验对象。视频序列的分辨率都为384×288,第一段视频序列中的跟踪目标为视频中出现的一个行人,跟踪从820帧开始到900帧结束,在820帧中由人工选定模板,大小为50×130像素,用矩形框来定位表示,实验中目标搜索区域的宽w设为32个像素,高h也设为32个像素。卡尔曼滤波的初始化为[10]:

矩阵A中dt=1,粒子群算法的种群大小设定为8,其中初始种群中一个点为卡尔曼预测的目标区域中心位置点,其他七个点在其周围随机选定。粒子群迭代总代数为30,c1=c2=1.496 2,惯性权重中wmax=0.9,wmin=0.4。设在一代群体中,如果适应度最高的个体其适应度函数值大于设定的阈值0.88就停止迭代,表示已搜索到目标。当粒子群算法迭代次数达到最大迭代次数时,如果所有个体适应度函数值较小,可推测目标被部分遮挡,当最优个体适应度函数值低于0.65,则可推测目标被严重遮挡,在这两种情况下,直接把卡尔曼滤波的预测结果作为最优值来处理。因文中采用的粒子群算法为连续优化算法,而图像像素位置为整数值,所以迭代过程中用floor函数对粒子的位置进行近似取整以对其候选区域精确化。实验表明此处理并不影响跟踪效果,结果如图3所示。

从图3可以看出跟踪目标与图像背景比较相似,这种情况下利用本文的算法还是取得了较好的跟踪效果,只是在处理部分帧的情况下需要一定的运算时间,但是在实验中,平均每秒可以处理10帧左右的图像,可以基本满足跟踪实时性的要求。

本文的粒子群与卡尔曼结合的算法还能有效处理部分遮挡情况,第二段视频序列从156帧到195帧,从中挑选几帧跟踪实验结果如图4所示。

图4的结果表明即使目标被部分遮挡,由于本文的算法在处理遮挡时用卡尔曼滤波预测值代替粒子群搜索最优值,可以保证跟踪能继续下去。

与穷举搜索算法相比,粒子群算法的匹配次数大大降低。在粒子群算法中,目标模板与候选区域进行匹配比较次数为迭代次数乘以种群数。该实验在最坏的情况下,比较次数为30×8=240次,如果采用穷举法,必须比较32×32=1 024次。在目标不被频繁遮挡的一般情况下,穷举法平均需要比较512次,而本文方法只需比较10多次。实验中还发现搜索区域的大小会影响跟踪效果,一般当运动目标运动速度快,方向变换频繁时,搜索区域设置要大。

4 结 语

本文运用卡尔曼滤波预测运动目标中心在下一帧中可能出现的位置,结合目标灰度统计特性,以候选区域与目标模板的相似度作为粒子群算法的适应值函数,实行精确搜索,使运算速度大大加快,基本满足了跟踪的实时性和鲁棒性,并能处理部分遮挡以及短暂的全部遮挡。如何解决多目标跟踪,以及更加复杂的遮挡问题,进一步提高跟踪实时性是本文下一步的研究方向。

参考文献

[1]COLLNS R,LIPTON A,KANADEAND T.A system forvideo surveillance and monitoring VSAM final report[R].Carnegie Mellon University,2000.

[2]李洪海.一种改进的CAMShift目标跟踪算法[J].现代电子技术,2010,33(16):106-108.

[3]胡建华,徐健健.一种基于遗传算法和卡尔曼滤波的运动目标跟踪方法[J].计算机应用,2007,27(4):916-918.

[4]纪震,廖惠莲,吴青华.粒子群算法及应用[M].北京:科学出版社,2009.

[5]赵树杰,赵建勋.信号检测与估计理论[M].北京:清华大学出版社,2008.

[6]胡炜薇,杨雷,杨莘元,等.粒子群算法在多传感器多目标跟踪的应用[J].哈尔滨工程大学学报,2007,28(1):102-107.

[7]姚禹.粒子群优化算法发展及应用[J].信息记录材料,2009,10(4):50-54.

[8]吕维,冯燕,李西风,等.一种基于运动轮廓的目标跟踪方法[J].计算机仿真,2009,26(12):172-176.

[9]COMANICIU D,RAMESH V,MEER P.Real-time track-ing of nonrigid objects using mean shift[C].IEEE Confer-ence on Computer Vision and Pattern Recognition,2000:142-149.

基于卡尔曼滤波的土壤水分同化试验 篇5

基于集合卡尔曼滤波的土壤水分同化试验

集合卡尔曼滤波是由大气数据同化发展的新的顺序同化算法,它利用蒙特卡罗方法计算背景场的误差协方差矩阵,克服了卡尔曼滤波需要线性化的模型算子和观测算子的.难点.我们发展了一个基于集合卡尔曼滤波和简单生物圈模型(SiB2,Simple Biosphere Model)的单点陆面数据同化方案.利用7月6日至8月9日青藏高原GAME-Tibet实验区MS3608站点的观测数据进行了同化试验.结果表明,利用集合卡尔曼滤波的数据同化方法可以明显地提高表层、根区、深层土壤水分的估算精度.

作 者：黄春林 李新 HUANG Chun-lin LI Xin  作者单位：中国科学院,寒区旱区环境与工程研究所,甘肃,兰州,730000 刊 名：高原气象  ISTIC PKU英文刊名：PLATEAU METEOROLOGY 年，卷(期)：2006 25(4) 分类号：P33 关键词：陆面数据同化系统   集合卡尔曼滤波   简单生物圈模型   土壤水分  

卡尔曼滤波算法 篇6

关键词：姿态测量； 微机电系统； 惯性传感器； 卡尔曼滤波

中图分类号： TP 275 文献标志码： A doi： 10.3969/j.issn.1005-5630.2015.01.007

Abstract：Micro-electronic mechanical system （MEMS） inertial sensors are usually used to measure the attitude of the small unmanned aerial vehicle （UAV）， in order to reduce the controlling cost and save the effective payload. In this paper， the Kalman filtering algorithm was implemented on MCU MC9S12XS128 by embedded software. The Data fusion of the MEMS accelerometer and gyroscope output signals was executed by Kalman filter. The experimental results show that the zero drift and mechanical vibration interference problem of the MEMS inertial sensors could be reduced dramatically by the Kalman filter.

Keywords：attitude determination； MEMS； inertial sensor； Kalman filter

引 言

卡尔曼滤波器是Kalman于1960年提出的一种应用于离散线性滤波的迭代算法[1]，其目的是尽可能地减少测量噪声的影响，并从含有噪声的测量值中得到系统状态的最优估计。在小型无人机自动驾驶仪的设计中，受有效载荷重量和安装体积的限制，业界多采用微机电系统（MEMS）惯性测量单元来获取无人机的飞行姿态参数[2-4]。同时，受MEMS传感器制造工艺的限制，由MEMS加速度计和陀螺仪组合而成的惯性测量系统不可避免地存在零漂误差，且测试数据极易受到机体振动的干扰。为此，本文着重对小型四轴飞行器纵摇和横摇姿态（分别对应俯仰角和横滚角）测量进行研究。

1 卡尔曼滤波器的构造

2 卡尔曼滤波实验与结果分析

如图1所示，采用三轴MEMS陀螺仪L3G4200D，三轴MEMS加速度计MMA8451Q和16位Freescale单片机MC9S12XS128构成惯性姿态测量单元。其中，加速度计和陀螺仪通过IIC总线与单片机GPIO口的E2、E3引脚相连，经卡尔曼滤波后的数据通过Tx、Rx引脚发送到PC机。

软件流程图如图2所示，流程包括传感器初始化、IIC数据采集、卡尔曼数据融合和串口数据发送等4个主要步骤。

首先将JZJ-1转台的俯仰角保持在22°附近，采样时间为50 s左右。在此期间用橡皮锤对转台底座施加机械冲击载荷，测试曲线如图3所示。由图3可见，MEMS陀螺仪输出的俯仰角度值随着时间的推移明显偏离实际值，这是典型的零漂现象。在机械冲击载荷作用的时刻，由加速度值换算得到的角度值会明显偏离实际值。而经卡尔曼滤波后输出的角度值零漂现象不明显，且对外界冲击载荷的抗干扰能力较强。

当转台从-5°附近开始产生动态俯仰运动，在测试过程中对底座施加1次锤击，测试曲线如图4所示。由图可见，在动态工况下，卡尔曼滤波后的动态角度曲线比单纯依靠加速度计测得的角度曲线更平滑且无偏移。此时，卡尔曼滤波器同样对机械冲击载荷有较好的抑制作用。

3 结 论

本文利用卡尔曼滤波器对MEMS陀螺仪和角速度计的输出信号进行了数据融合，较好地解决了零漂和机械振动干扰问题。其基本原理是利用MEMS陀螺仪的信号作为预测值，利用加速度计输出信号对其校正。所设计的卡尔曼滤波器可运行在16位飞思卡尔单片机上，受单片机浮点运算能力的限制，采样时间间隔设置为0.1 s。如何在32位ARM芯片上实现多通道卡尔曼滤波，并提高采样频率将是今后研究工作的重点。

参考文献：

[1] 秦永元，张洪钺，汪叔华.卡尔曼滤波与组合导航原理[M].西安：西北工业大学出版社.1998：110-135.

[2] 付勇杰，丁艳红，梁义维，等.动态倾角传感器及其传递特性的研究[J].仪表技术与传感器，2012（9）：6-8.

[3] 周升良，孙玉国，任强.基于MEMS的摇摆振动测试方法研究[J].信息技术，2014（1）：15-16.

[4] 任强，周升良，孙玉国.基于MEMS的角位置无线随动控制系统设计[J].传感器与微系统，2013，32（9）：122-124.

（编辑：刘铁英）

卡尔曼滤波算法 篇7

环境污染是世界范围普遍关心的问题。对各类燃烧器、工业及商用锅炉产生的气体、机动车尾气、各类化工厂有毒气体(如HCl、HF)等进行环境污染物的监测和预报,是保护环境、控制治理污染的必要前提条件[1,2]。传统的气体污染监测是以湿式化学技术和吸收取样后的实验分析为基础,这些仪器通常只限于单点测量,响应时间比较慢。相比而言,光学和光谱学遥感技术以其大范围、连续实时监测方式而成为环境气体监测、分析的理想工具。其中差分吸收光谱技术DOAS(Differential Optical Absorption Spectroscopy)[3,4,5,6,7,8]以其出众的测试方法及技术特点成为大气污染模式研究和大气污染监测的常用方法之一[2]。DOAS技术中一般采用最小二乘原理对气体浓度进行反演,但是这种方法的缺点的在反演求解过程中假设系统数据(如压强、温度等)精确已知。而实际情况是系统往往会受到各种因素的扰动,因此造成该反演算法的鲁棒性不够,而且在数据含有大噪声的情况下反演精度也不够理想。

为了解决上述问题,针对实际应用中可能遇到的各种扰动因素,把气体浓度随时间变化的方程建模成状态方程,从DOAS测量原理出发推导出测量方程,从而建立浓度气体反演的状态空间表达,随后充分考虑系统的不确定性及其测量数据的噪声问题,采用卡尔曼滤波改进原最小二乘算法的不足。

2 方法

2.1 DOAS测量原理

DOAS光谱测量基本原理:已知从光源发出的光束经过某一待测气体,由于不同气体分子对光的吸收不同,使得吸收光谱的强度和结构都发生了相应的改变。提取这些特征结构,然后利用最小二乘法,就可以求得相应气体的浓度。这就是DOAS方法的主要思想[2,3]。光强分布服从Beer-Lambert公式

式中:λ为波长,I(λ)为吸收光谱的光强测量值,I0(λ)为出射光光强初始值,L为光程,c为待测气体浓度,σ(λ)为气体吸收截面,εR(λ)为Reileigh散射系数,εM(λ)为Mie散射系数,N为各种噪声总和。

对(1)式两边取对数得:

通过光谱仪测量,可以测得对应波长λk(k=1,2,…,n)的强度I(λk)和I0(λk),然后采用最小二乘法对这n组超定方程求解浓度c。

最小二乘法只对量测值和相应的估计值σ(λk)Lĉ之间的误差的平方和求最小,而且在求解过程中气体吸收界面数据来自先验的实验室测量,而没有考虑到实际系统环境中温度及其压强的变化,这是最小二乘方法的局限之一。其次,在噪声较大的情况下,这就会使得浓度估计值大大的偏离了真实浓度值,造成反演精度下降。因此通常的做法是首先对方程(2)中的噪声进行滤除以降低噪声的干扰,然后再用最小二乘法求解,这样便可进一步提高反演的精确度。但是在噪声滤除的过程中,往往采用多项式拟合来代替低频部分,这就造成了拟合误差,而且噪声的特性也是未知的,高通滤波的噪声滤除方式也是不精确的。

为此,本文同时从噪声统计和考虑系统不确定性的角度,利用状态估计值和量测值不断对浓度状态值进行估计修正,从而提供反演结果的鲁棒性和精度。

2.2 基于Kalman滤波的气体浓度反演

首先我们假设气体浓度达到稳态的时候,不随时间变化,有:

对其进行离散化,得到浓度方程变为

式中wk表示对系统建模的不确定性。根据式(2),我们对其进行简化,令Hk=σ(λk)L,vk=εR+εM+N′,从而得到测量方程:

式(4)和式(5)就构成了气体浓度反演的状态空间表达,假定动态噪声{wk}和量测噪声{vk}是不相关的零均值白噪声序列,并且符合以下统计特性:

通过对标准卡尔曼滤波的求解[9],得到如下解:

在稳态条件下,状态方程中的状态矩阵Ak,k-1为单位矩阵。

从式(7)~(11)中可知,标准卡尔曼滤波模型要求系统的动态噪声和测量噪声为零均值且统计特性已知的白噪声,但是在实际气体浓度测量中,存在着Reileigh散射、Mie散射以及光电噪声等噪声,不可能准确地测得动态噪声的统计特性。

针对标准卡尔曼滤波的缺点,Mehra提出了利用信息序列的自适应估计开窗逼近法[10],来估计测量方程协方差阵及状态误差协方差阵。它的基本原理是利用历史信息和测量残差来估计当前状态噪声协方差矩阵和测量噪声协方差矩阵,从而使状态协方差矩阵和测量协方差矩阵趋向于实际噪声协方差,并且能自适应于当前状态信息和测量信息,再应用标准卡尔曼滤波模型进行计算,获得系统状态的最优估值[11,12]。

现在假定实际噪声一般噪声,其模型为

设状态信息为

测量信息为

利用前N次的历史信息,则可得噪声的协方差。

式(15)~(18)利用历史信息估计了状态噪声和测量噪声的特性,使得卡曼滤波能在不确定的环境下,估计浓度状态值更加接近真实值。

3 实验及结果

图1为实验室测量臭氧气体浓度的装置示意图。氙弧灯光源发出的光到达准直透镜,准直后的平行光通过装有被测量气体的气体池后,出射光经会聚透镜会聚到传输光纤上,经光纤传输到达光谱仪,光谱仪利用光栅将接收到的光展开成光谱带,通过CCD对光谱进行扫描并将光信号转换为电信号,经A/D转换卡数字化后送入计算机存储或处理。

通过计算机采集到的原始(参考光谱)、吸收光谱(测量光谱)等数据,如图2所示,再通过上述的DOAS和卡尔曼滤波可以对气体浓度实现反演。同时也可在实验室条件下改变待测气体的浓度、压强、温度,来检验两种方法的优劣。

1)外部噪声改变时候

在外部噪声变化时,我们比较在不同信噪比的条件下,两种方法的所求得浓度的误差。

表1的结果表明卡尔曼滤波在信噪比较低的条件下,比DOAS效果要好,更能适应实际工作环境。

2)系统环境变化

当外界压强温度变化时,系统的吸收截面也会发生相应的改变,也就是系统矩阵发生变化。当继续以标准吸收截面来反演浓度时,就会带来较大的误差。从表2可以看出,卡尔曼的结果比DOAS更精确。

最后,我们采集了大量的光谱数据,与传统的方法进行了详细的比较,部分结果列入表3中,很显然,我们提出的方法与传统的方法相比都得到了比较好的结果。

4 结论

本文分析了差分吸收光谱(DOAS)最小二乘算法存在的不足,并在DOAS的基础上引入了卡尔曼滤波算法。卡尔曼滤波能有效提高浓度状态反演对噪声的适应性,更适合于复杂环境下的气体浓度探测。

摘要：广泛应用于气体探测的差分吸收光谱技术(DOAS)利用气体分子的窄带吸收特性结合最小二乘算法来推演气体浓度。但是,最小二乘法在外界环境因素干扰的情况下,往往产生较大的误差。本文引入了基于状态空间理论的气体浓度定量分析算法。通过把浓度变化视为状态方程,把光强吸收变化看作测量方程,从而组成状态空间方程,然后将卡尔曼滤波应用到气体状态空间中实现浓度反演。对于噪声统计信息未知的情况,通过自适应滤波算法,在滤波过程中利用已有的历史信息对噪声实现估计,从而使得整个系统在信噪比较低的情况下也能取得较好的反演精度。最后通过实验对最小二乘算法和卡尔曼滤波算法进行对比,证明卡尔曼滤波算法更具优越性。

关键词：差分吸收光谱技术,卡尔曼滤波,最小二乘法

参考文献

[1]张世森.环境监测技术[M].北京:高等教育出版社,1992.ZHANG Shi-sen.Environment Monitoring Technology[M].Beijing:Higher Education Press,1992.

[2]庄马展,吴宇光,杨青.差分光谱仪与传统点式仪器测定环境空气质量对比研究[J].环境保护,2000(5):25-27.ZHUANG Ma-zhan,WU Yu-guang,YANG Qing.Study on monitoring of ambient air by DOAS and conventional monitors[J].Monitoring and Assessment,2000(5):25-27.

[3]Platt U,Perner D.Simultaneous measurement of atmospheric CH2O,O3and NO2by differential optical absorption[J].Geophys Res,1979,84:6329-6335.

[4]Platt U,Perner D.Direct measurements of atmospheric CH2O,HNO2,O3,NO2,SO2by differential optical absorption in the near UV[J].Geophys Res,1980,85:7453-7458.

[5]Platt U,Perner D.Detection of NO3in the polluted troposphere by differential optical absorption[J].Geophys.Res.Lett.2001,7:89-92.

[6]Edner H,Sunesson A,Svanberg S.Differential optical absorption spectroscopy system used for atmospheric mercury monitoring[J].Applied Optics,1986,25(3):403-409.

[7]Plane J M C,Nien C F.Differential optical absorption spectrometer for measuring atmospheric trace gases[J].Rev Sci Instrum,1992,63(3):1867-1876.

[8]Evangelisti F,Baroncelli A.Differential optical absorption spectrometer for measurement of tropospheric pollutants[J].Applied Optics,1995,34(15):2737-2744.

[9]Edward W,Su J K.Introduction to Optimal Estimation[M].London:Springer,1999.

[10]Mohamed A H,Schwarz K P.Adaptive Kalman filtering for INS/GPS[J].Journal of Geodesy,1999,73:193-203.

[11]Glad T,Ljung L.Control Theory[M].London:Taylor&Francis,2000.

卡尔曼滤波算法 篇8

关键词：组合导航,制导炸弹,卡尔曼滤波

0 引言

由于MINS和GPS两者都是全球、全天候和全时间的导航设备, 而且均可以提供十分完整的导航数据, 两者的结合可以达到优势互补的效果:对惯导系统可以实现惯性传感器的校准、惯导系统的海上对准、惯导系统的高度通道稳定等目的, 从而可以有效的提高惯导系统的性能和精度;对于GPS全球定位系统, GPS接收机在高动态环境或障碍物遮挡情况下容易发生卫星信号的短期失锁, 从而造成无数据输出或输出数据不更新的问题, 惯导系统的辅助可以提高接收机的动态特性和抗干扰性。

本文提出了一种混合卡尔曼滤波算法, 建立了MINS/GPS组合导航系统的非线性误差模型, 分别给出了系统的状态方程和量测方程, 尝试将此算法应用于INS/GPS组合导航系统估计中以提高导航定位精度和收敛速度。最后通过实验进一步验证了混合卡尔曼滤波在组合导航系统应用中的有效性。

1 系统的状态模型

下面建立组合导航系统的状态方程, 包括姿态误差方程、速度误差方程和位置误差方程。

姿态误差方程为

速度误差方程为

位置误差方程为

其中, δV E、 δVN 分别为东向和北向的速度误差。δL、δλ 分别为纬度和经度误差, Re 为地球椭球长半轴, f为地球椭球扁率, ωie 为地球自转角率。

非线性模型中状态向量取为

则公式 (1) ~ (5) 构成了MINS非线性状态方程。

2 基于混合卡尔曼滤波的MINS/GPS组合导航算法研究

在时刻k , 首先用UKF更新粒子, 以获得相应的状态估计值然后, 分别计算系统模型与测量模型的雅可比矩阵, 用EKF更新粒子, 此时使用UKF已经得到的状态估计值作为k?1时刻的状态估计, 即, 令经过计算得到k时刻最终的状态及其相应的协方差的估计值。从而可以从建议分布中抽取粒子。

混合卡尔曼滤波算法步骤如下:

2.1初始化

从先验概率密度p (X0) 中求初始状态

k=1, 2… i=1, 2, …, N

2.2 重要性采样:

(1) 按式 (4.40) ~ (4.42) 进行UT变换, 计算每个采样点

(2) 选择重要性密度函数:

(3) 按 (4.51) 式计算未归一化粒子权值

(4) 按 (4.45) 式对粒子权值进行归一化得到

2.3 重采样

(1) 按 (4.47) 式计算

(2) 如果

按重采样

2.4 输出

该算法的输出为粒子集合及相应的权值

在MMSE准则下状态的最优估计为

3 非线性滤波结果及分析

MINS/GPS组合导航滤波均方误差均值与方差如表1所示。

由计算结果可以看出:

(1) 在某些时刻EKF产生的估计偏离真实值较为严重, 惯性陀螺随着时间的积累, 非线性越来越严重, 模型的线性化误差也逐渐加大, 从而导致EKF的估计精度下降。而其他算法不需要对系统的观测方程进行线性化, 因此估计精度不受线性化误差的影响。

(2) UKF和EKF在经度、高度方向上误差相近, 在纬度方向上UKF和EUKF的精度接近, 但EUKF效果的最为理想。一方面说明了在一般的非线性高斯环境中, 如果采用UKF方法, 只要合理确定参数值, 就可能获得对状态均值和方差的估计误差在4阶水平上, 能获得较好的滤波精度, 且运算较快。

(3) EUKF首先用Unscented卡尔曼滤波器产生系统的状态估计, 然后用扩展卡尔曼滤波器重复这一过程并产生系统的最终状态估计。它可用于处理非线性非高斯模型, 对系统的过程噪声和量测噪声分布要求不高, 而且算法简单, 避免了UKF受粒子数影响而程序变慢的缺点, 只是还不能完全消除跟踪定位过程中不确定噪声的干扰。

4 结论

本章首先建立了精确制导炸弹导航定位的数学模型, , 提出了一种新的滤波算法——混合卡尔曼粒子滤波。实验结果表明:UKF算法比EKF有更高的跟踪精度, 但EUKF算法和实际轨迹最接近, 滤波效果明显优于其他滤波。通过对制导系统的数据处理, 提高了系统的稳定性, 减小了定位误差。

参考文献

[1]Grewal M S, Henderson V D, Miyasako R S.Application of Kalman Filtering to theCalibration and Alignment of Inertial Navigation Systems[J].IEEE Trans.on Automatic Control, 1991, 36 (1) :4-13.

[2]Julier S J.A new method for the nonlinear transformation of means and covariance in filters and estimators[J].IEEE Trans on Automatic Control, 2000, 45:477~482

[3]王中训, 管旭军, 王德法.非线性滤波算法在无源定位中的应用[J].烟台大学学报 (自然科学与工程版) , 2007, 20 (1) :35~39

[4]Julier S J, Uhlmann J K, Durrant-Whyte H F.A new approach for filtering nonlinear systems[A].In Proc.of the American Control Conference[C], Seattle, Washington, 1995:1628-1632.

[5]Julier S.J., Uhlmann J.K..Unscented filtering and nonlinear estimation[J].Proceedings of the IEEE, 2004, 92 (3) :401~422

[6]Hao Y, Xiong Z, Wang W, Sun F.Rapid transfer alignment based on unscented KalmanFilter[A].In Proc.of the 2006 American Control Conference[C], Minneapolis, USA.2006:2215-2220.

[7]Zhan.R, J.W.Neural network-aided adaptive Unscented Kalman filter for nonlinear state Estimation[J].Signal Processing Letters, IEEE, 2006, 13 (7) :445-448.

卡尔曼滤波算法 篇9

关键词：红外目标,跟踪,卡尔曼滤波

0 引言

目前, 红外成像系统的被动工作方面的明显优势已使其成为现代制导系统中的一项实用关键技术[1]。近年来, 复杂背景下的红外目标探测与跟踪技术在军事、民事领域均获得了广泛应用, 国内外专家学者对基于贝叶斯理论的各种算法也表现出了很高的热情, 越来越多的研究人员将这类算法与现有的检测跟踪算法结合, 以此提升跟踪的精度和准度。

1 KF算法

卡尔曼滤波是最佳线性滤波的一种, 具体来说属于线性最优均方误差估计滤波器, 并被广泛应用于导航、控制、传感器、数据融合、雷达系统等军事和民事领域。卡尔曼滤波理论是在克服了维纳滤波使用大量过去的数据, 无法满足实时处理要求的基础上提出的一种合理有效的最优化自回归数据处理算法[2]。卡尔曼滤波 (KF) 对其需要求解的系统有如下要求:线性系统、高斯噪声、高斯分布的概率密度函数[3]。

上式为系统的状态方程, 其中x (k) (N×1) 为k时刻系统的状态向量, F (N×N) 为状态转移矩阵, 描述动态系统k-1时刻到k时刻状态的转移, v1 (k) (N×1) 为过程噪声。公式 (2) 为系统的观测方程, 其中为k时刻系统的观测向量, H (M×N) 为观测矩阵向量, v2 (k) (M×1) 为观测噪声向量。同时, v1 (k) 和v2 (k) 分别为相互独立的正态高斯白噪声, 预测和更新公式如下:

从上式中可以看出, 卡尔曼滤波器的状态控制是通过递推和反馈实现的, 不断的预测和更新, 最终获得最优的线性滤波估计。卡尔曼滤波器 (KF) 是基于高斯分布的状态预测方法, 能够实现最优的线性滤波估计。

3 分类背景预测

原始的红外视频图像经过边缘检测得到边缘图像、非边缘图像:边缘图像通过局部最相似算法获得边缘背景[4], 非边缘图像通过四方向的最大均值滤波获得静态背景[5], 预测背景由静态背景和边缘背景融合而成。该算法还包括一个预处理过程:全局搜索﹑图像分块和边缘检测三个部分, 流程图1所示。

4 算法实现

在红外目标识别中, 30像素值以下的目标统称为小目标[6]。无论目标的大小, 由于红外热成像原理的特殊性, 红外目标通常表现为本身红外热辐射强度较高且为灰度的奇异点。它们与周围背景部分的相关性较弱, 在实际的监测跟踪过程中容易跟丢[7]。本文在分类背景预测算法的基础上利用卡尔曼算法实时预测更新, 最终获得较为优异的跟踪效果:将第k-1帧的估计结果代入预测公式, 获得第k帧的预测结果;以为中心点设置跟踪子框, 具体尺寸由实际目标尺寸决定;在子框中进行分类背景预测, 预测结果记为第k帧的观测值Zk;将第k帧的预测值和观测值代入估计公式, 获得第k帧的估计结果。

5 实验仿真

5.1 场景设置

为了更好的验证算法性能, 将本算法运用到实际采集的红外目标图像序列中, 红外视频序列的采集均使用武汉高德的红外热像仪 (TP8) , IRV格式输出。红外序列每帧图像的大小为288×348像素, 图像序列分别为复杂背景遮挡环境和行人环境。

5.2 仿真及结果分析

复杂背景且遮挡环境下红外目标图像序列的结果如图2所示, 其中红色方形跟踪框为卡尔曼预测子框, 红点为估计位置。此处给出了红外视频序列中第15帧、55帧、85帧、105帧、205帧、305帧的跟踪结果。从序列Ⅰ的处理结果可以看出:目标在第1到105帧的运动过程中受到了障碍物的遮挡, 从105帧开始目标远离障碍物。本算法跟踪结果基本克服了遮挡问题, 跟踪位置较为准确。

行人环境下的红外图像序列的跟踪结果如图3所示:其中红色跟踪框为的卡尔曼预测子框, 红点为估计位置。从序列Ⅱ的处理结果可以看出在非小目标环境中, 跟踪框的位置始终保持在目标头部附近, 且没有跟丢的情况出现, 算法均获得了较好的跟踪效果。

6 结论

本文基于贝叶斯的跟踪思想, 将卡尔曼预测估计与分类背景预测的检测思想相融合, 获得一种目标跟踪算法:子框局部搜索跟踪算法。实验表明, 本算法能够应对复杂及遮挡背景的跟踪环境, 更适合于实际运用中, 背景信息不明确、环境状况无法确定的红外目标跟踪场合。

参考文献

[1]汪震, 乔延利, 洪津, 王峰, 李文涛.利用热红外偏振成像技术识别伪装目标[J].红外与激光工程, 2007, 36 (6) :854-855.

[2]张迎春, 李璟璟, 吴丽娜, 李化义.模糊自适应无迹卡尔曼滤波方法用于天文导航[J].哈尔滨工业大学学报, 2012, 44 (1) :13-15.

[3]朱斌, 樊祥, 马东辉.一种改进的自适应背景预测红外弱小目标检测算法[J].激光与红外, 2007, 37 (7) :683-686.

[4]郭张婷, 辛云宏.基于分类背景预测与图像分块技术的红外小目标检测算法[J].激光与红外, 2012, 42 (5) :572-578.

[5]徐军.红外图像中弱小目标检测技术研究[D].西安电子科技大学, 2003:32-37.

[6]Xin Wang, Zhenmin Tang.Modified particle filter-based infrared pedestrian tracking[J].Infrared Physics&Technology, 2010, 10 (3) :122-124.

卡尔曼滤波算法 篇10

本文综合考虑了最小二乘法和常规卡尔曼滤波的优缺点,提出了一种改进的卡尔曼滤波算法。该算法利用最小二乘法提供接收机的初始坐标,然后利用卡尔曼在已经得到的初始坐标的基础上进行滤波。同时,本文对卡尔曼滤波做了适当的修改,即不直接利用卡尔曼滤波估计接收机的状态,而是估计状态的误差,使舍入和非线性误差达到最小,同时在矩阵运算中由于将许多元素设为零,减少了运算量,提高了系统的实时性和运算精度。

1 标准卡尔曼滤波模型

卡尔曼在1960年提出的卡尔曼滤波算法是一种线性最小方差估计方法。他通过建立式(1)中的系统方程和量测方程来描述系统的动态变化过程,依据滤波增益矩阵的变化,从测量数据中提取有用信息,修正状态参量,无需存储不同时刻的观测数据,便于实时数据处理。

undefined

根据上述建立的卡尔曼滤波方程,给定滤波初值和相关噪声统计特性,滤波过程如下[3]:

undefined

2 运动载体的GPS动态定位系统数学模型

在建立卡尔曼滤波方程时,需要知道运动载体的GPS动态定位系统数学模型。

根据接收机在k时刻与k-1时刻的关系,应用Taylor级数展开并略去高次项(具体的展开阶数根据精度来定,一般情况下展开到三阶导数),得到如下的数学模型:

undefined

在模型(3)中,各变量的含义如下:δx,δy,δz表示3个方向的位置坐标误差;δvx,δvy,δvz表示速度误差;δax,δay,δaz表示加速度误差;δjx,δjy,δjz表示加加速度误差;b表示接收机时钟偏差;δb表示接收机时钟漂移误差,T表示采样周期。

3 提高GPS定位精度的改进卡尔曼滤波算法的实现

在进行卡尔曼滤波时,关键是确定状态向量、建立系统方程和量测方程、确定噪声统计特性等。我们将在下面的部分中,分别加以讨论。

3.1 系统的状态向量

根据上面推导的系统数学模型式(3),选取如下的系统状态向量和系统噪声向量:系统状态向量选取为X=[δx δvxδy δvyδz δvzb δb],将加速度、加加速度转化为随机噪声,用白噪声来描述;系统噪声向量W=[δaxδjxδayδjyδazδjz]。

3.2 系统方程的推导

根据系统的误差状态向量、系统噪声向量和数学模型(3),得到的系统方程如下:

undefined

其中:

undefined

3.3 量测方程的推导

量测方程矩阵的元素把观测量和状态向量联系起来,本方法采用伪距做为观测量。为完成观测量和状态向量的关联,需要把每次接收到的测量值转化为位置误差、速度误差以及接收机时钟差,然后把这些误差分解到其状态矢量的分量中。

应用伪距作为量测值进行解算时,伪距方程组[1,2,7]:

undefined

其中,b=cΔt,wj=ρj+c(Δtj-Δtundefined-Δtundefined-Δtundefined);分别用Δtj,Δtundefined,Δtundefined,Δtundefined表示第j颗卫星的钟差、电离层延迟、对流层延迟、多路径延迟。应用Taylor级数在接收机的概略坐标处展开,并略去高次项,得到:

undefined

其中:undefined

undefined

在应用卡尔曼滤波进行数据处理时,我们对上面的wj进行修改,近似认为wj=ρj,将式(6)中wj的其他项都转化到伪距的测量误差中,得到的结果如下:

undefined

设视界内的卫星数为N,将上述方程组写为矩阵形式,同时考虑到相应的伪距测量误差,得到量测方程:

undefined

其中,Z(k)=[L1L2 … LN]T,V(k)为量测噪声序列,H(k)为量测矩阵;

undefined

3.4 噪声统计特性的确定

系统噪声和量测噪声都是零均值的高斯白噪声序列[1,3,4,5]。系统噪声方差阵Q的主对角元素构成的向量为[δundefinedδundefinedδundefinedδundefinedδundefinedδundefined],量测噪声方差阵R的主对角元素构成的向量为[δundefinedδundefined… δundefined],方差阵Q与R的其他元素为零。其中:δundefined表示伪距的噪声方差,δundefined表示伪距率的噪声方差。

3.5 滤波初值的选取

卡尔曼滤波是递推算法,需给定状态向量初始值X(0)与初始估计均方误差矩阵P(0)。在不了解系统特性的情况下,常令X(0)=0,同时令初始估计均方误差矩阵P(0)=αI(其中α是很大的正数,I为单位矩阵),这样可以保证滤波的结果是无偏的[3]。

4 仿真分析

仿真条件为:采样周期T=1 s,常数α=1 000 000.0,伪距噪声方差δD2= 100.0 m2,伪距率噪声方差δd2=25.0 m2/s,初始的状态向量X(0)=0。用于定位解算的卫星的位置坐标和伪距来源于文献[7]。图1给出了沿X轴方向的卡尔曼滤波定位解算的位置误差曲线,图2给出了沿x轴方向的卡尔曼滤波定位解算的速度误差曲线。

从两图可以看出,采用估计接收机载体状态误差的方法大约经过10次迭代,位置和速度就能够稳定而且精度很高。随着卡尔曼滤波的不断进行,位置和速度误差逐渐降低,最后趋近于零。仿真结果证明了此方法具有较高的定位精度和较快的收敛速度。

5 结 语

针对最小二乘法以及常规卡尔曼滤波算法的不足,本文研究了的改进卡尔曼滤波算法。该算法结合了最小二乘法与卡尔曼滤波算法的优点,采用伪距作为观测量,不直接估计载体的状态,而是估计载体的状态误差从而对载体的状态进行估计,这样使估计误差达到最小。以上的理论分析和仿真结果表明:研究的改进卡尔曼滤波定位算法是合理的,对滤波模型的建立、状态向量和噪声向量的选取、系统噪声方差阵和量测噪声方差阵等问题的处理方法是可行的。此算法滤波过程稳定,具有较快的收敛速度和较高的定位精度。

参考文献

[1]张守信.GPS技术与应用[M].北京:国防工业出版社,2004.

[2]刘立龙.动态对动态GPS高精度定位理论及其应用研究[D].武汉:武汉大学,2005.

[3]秦永元,张洪钺,汪叔华.卡尔曼滤波与组合导航原理[M].西安:西北工业大学出版社,1998.

[4]帅平,陈定昌,江涌.关于GPS导航计算的卡尔曼滤波问题[M].遥测遥控,2001,22(5):14-20.

[5]董绪荣,陶大欣.一个快速Kalman滤波方法及其在GPS动态数据处理中的应用[J].测绘学报,1997,26(3):221-227.

[6]Euler H,Goad C C.On Optional Filtering of GPS Dual-Frequency Observations without Using Orbit Information.Bulletin Giodisique,1991,65(2):130-143.