Unscented Kalman filter and its nonlinear application for tracking a moving target

UKF在Matlab下的实现西安交通大学刘继冬1.实验综述实验综述：：无迹卡尔曼滤波UKF 是重要的非线性滤波方法。

它采用UT 变换的方法，不再近似系统的非线性方程，它仍然用高斯随机变量表示状态分布，不过是用特定选择的样本点加以描述，每个点叫一个高斯点，它从系统状态的概率密度函数中取出；然后，按系统的真实模型演化，得到非线性演化后的σ点，使得样本均值和样本方差是真实均值和真实方差的好的近似。

在这次实验中，实现了基于UKF的滤波方法，并且建立了两种仿真环境进行实验。

2.2.仿真环境设置仿真环境设置仿真环境设置：：（1）仿真仿真环境环境环境11：一维的线性直线匀加速运动，理想的初始速度为0，位置为0，加速度为1。

速度，位置和加速度均有一定的误差，且误差可能较大，误差系数约为0.3。

同时，测量系统只能检测到当前的速度，其余是0，而且干扰较大。

系统的状态方程为：11t 0X X Q 01t 001K K K +=+测量方程为：[]1Z 010X R K K K +=+状态变量X=（x1，x2，x3）T ，其中，x1为位置，x2为速度，x3为加速度。

（2）仿真仿真环境环境环境22：二维平面的匀加速运动，系统的初始状态为：处于（0.3，0.2）位置，初始速度为X轴为1，Y轴为2，加速度为X方向为2，Y方向为-1。

测量方程为非线性。

若简化为线性系统，为了满足能观性，选取可以测量的量不仅为当前的速度，也有当前的位置和加速度。

状态方程为：110t 000010t 00X X Q 00010t 000010000001K K K +=+测量方程为：1Z h(X )R K K K +=+h=[(x(1)+1)^0.5;0.8*x(2)+0.3*x(1);x(3);x(4);x(5);x(6)] 状态变量X=（x1，x2，x3，x4，x5，x6）T ，其中，x1为X轴位置，x2为Y轴位置，x3、x4分别X，Y轴的速度，x5、x6为两方向的加速度。

闪烁噪声下的自适应无迹卡尔曼算法陈政;刘康【摘要】针对非高斯噪声环境下过程噪声统计特性未知时机动目标跟踪精度不高甚至发散的问题,提出了一种应用EM算法和无迹卡尔曼滤波(Unscented Kalman filter,UKF)相结合的方法.该方法借助EM算法估计出较准确的过程噪声参数,再使用无迹卡尔曼滤波算法获得高精度的目标运动状态.仿真实验结果表明,该方法可以有效抑制滤波发散并显著提高跟踪精度.【期刊名称】《软件》【年(卷),期】2018(039)001【总页数】5页(P70-74)【关键词】EM算法;无迹卡尔曼;跟踪精度【作者】陈政;刘康【作者单位】昆明理工大学信息工程与自动化学院,云南昆明 650500;昆明理工大学信息工程与自动化学院,云南昆明 650500【正文语种】中文【中图分类】TP8010 引言卡尔曼最初提出的滤波基本理论只适用于线性系统，并且要求量测也必须是线性的[1]。

卡尔曼算法可以看成是一种贝叶斯方法，贝叶斯方法通过对模型引入先验分布，并利用似然函数求解后验分布的方法来对模型中的不确定性进行建模[2]。

在线性估计算法的发展历程中，卡尔曼滤波扮演了重要的角色，但该算法实现的前提是系统模型已知，与实际工业中的状态估计问题不相符[3]；同时，真实环境中卫星雷达受到的非高斯闪烁噪声进一步加重了后验概率密度的非高斯特性[4]，于是，传统的卡尔曼滤波已经不再适用[5][6]。

由此带来了改变后的使用到非线性系统观测中的滤波理论，如无迹卡尔曼滤波器（UKF），可用于导航、目标状态估计、飞行器轨道确定等领域并获得了良好效果，但噪声统计特性未知时，UKF滤波精度下降[7]。

全球卫星导航系统在许多领域都发挥出了重要的作用，并且仍然在不断地改进与创新[8]。

近年来，随着我国空间技术的发展，在轨运行的卫星越来越多[9]。

卫星侦察范围广，可对目标进行长时间、大范围的连续侦察和监视，获取情报的时效性强，是现代侦察中不可或缺的重要手段[10]。

卡尔曼滤波原理详解The Kalman filter is an algorithm used for estimating the state of a linear dynamic system based on a series of noisy measurements. 卡尔曼滤波是一种用于估计线性动态系统状态的算法，其基于一系列带有噪声的测量值。

It was developed by Rudolf Kalman in the 1960s and has applications in a wide range of fields including engineering, economics, and navigation. 这个算法是由鲁道夫·卡尔曼在20世纪60年代开发的，广泛应用于工程、经济学和导航等领域。

The Kalman filter works by recursively updating an estimate of the system's state based on new measurements and a model of the system dynamics. 卡尔曼滤波通过根据新的测量值和系统动态模型递归更新系统状态的估计值。

It combines information from the measurements and the model to produce an optimal estimate that minimizes the mean squared error. 它将测量值和模型的信息结合起来，产生一个最优估计，以最小化均方误差。

One of the key advantages of the Kalman filter is its ability to handle noisy and incomplete measurements. 卡尔曼滤波的一大优点是它能够处理带有噪声和不完整的测量值。

基于无迹卡尔曼滤波和权值优化的改进粒子滤波算法冉星浩;陶建锋;杨春晓【摘要】针对传统粒子滤波面临的重要密度函数的选取和粒子多样性丧失引起的样本贫化问题,提出基于无迹卡尔曼滤波和权值优化的改进粒子滤波算法.与传统的粒子滤波算法相比,有两点改进:首先该算法采取无迹卡尔曼滤波产生建议分布函数;其次,在重采样过程,提出基于权值优化的改进重采样算法来增加粒子的多样性.仿真结果表明,改进算法降低了粒子滤波算法的粒子退化程度并避免样本贫化现象的出现,更加接近真实值,提高了跟踪精度.%Aiming at the problems of the importance function choice and the sample impoverishment after resampling, an improved particle filter algorithm was proposed in this paper,which based on the unscented Kalman filter and weight pared with the traditional particle filter,this algorithm had two improvements,UKF was used to gen-erate the importance density function,and weight optimization was used to ensure all useful information inherited, which could maintain the diversity of particle.The theory analysis and simulation showed that the improved particle fil-ter algorithm could solve particle degeneracy and avoid sample impoverishment.【期刊名称】《探测与控制学报》【年(卷),期】2018(040)003【总页数】6页(P74-79)【关键词】粒子滤波;无迹卡尔曼滤波;权值优化;样本贫化【作者】冉星浩;陶建锋;杨春晓【作者单位】空军工程大学防空反导学院,陕西西安 710051;空军工程大学防空反导学院,陕西西安 710051;中国人民解放军93567 部队,河北保定 074100【正文语种】中文【中图分类】TN7130 引言相比于传统的卡尔曼滤波算法，粒子滤波算法不受线性化误差或高斯噪声假定的限制，适用于非线性、非高斯系统。

平方根体积积分卡尔曼滤波郭文艳;姬春艳;周吉瑞【摘要】Cubature quadrature rule is a kind of higher algebra precision integration method to improve nonlinear state estimate problems. To enhance the stability of nonlinear filter, based on a square root decomposition of covariance matrix, a new Square Root Cubature Quadrature Kalman Filter(SRCQKF)is proposed. The spherical radial cubature rule and Gauss-Laguerre quadrature rule are applied to compute the integral points. The square root of covariance matrix is obtained and propagated by matrix QR decomposition. Two typical nonlinear examples show that the SRCQKF improves the accuracy of nonlinear estimation and has more stability.%体积积分是一种新的具有较高代数精度的积分方法。

为了提高非线性滤波算法的精度和数值稳定性，将体积积分规则和平方根分解引入卡尔曼滤波框架中，提出了平方根体积积分卡尔曼滤波算法（SRCQKF）。

新算法采用球半径体积规则和高斯-拉盖尔积分规则计算积分点，利用矩阵的QR分解得到协方差矩阵的平方根并传播平方根。

两个典型的非线性系统的实验结果表明，与体积卡尔曼滤波相比，新算法提高了非线性状态的估计精度，具有较高的数值稳定性。

基于Kalman滤波器的车式移动机器人跟踪方法张香竹;余辉荣;汪俊彬【摘要】为了跟踪和优化车式移动机器人的运动轨迹,对四轮车式机器人进行了建模,再通过线性化和离散化得到系统模型,并对该模型引入Kalman滤波器.搭建实验平台,详细验证Kalman滤波算法对多目标跟踪的优化和平滑作用.实验结果验证了多个Kalman滤波器对多目标进行实时轨迹和状态跟踪的有效性.【期刊名称】《自动化与仪表》【年(卷),期】2014(029)002【总页数】4页(P1-4)【关键词】车式移动机器人;建模;Kalman滤波器;多目标跟踪;轨迹优化【作者】张香竹;余辉荣;汪俊彬【作者单位】华南理工大学自动化科学与工程学院控制与优化中心,广州510641;华南理工大学自动化科学与工程学院控制与优化中心,广州510641;华南理工大学自动化科学与工程学院控制与优化中心,广州510641【正文语种】中文【中图分类】TP242.6目前，在智能监控领域，对视频序列中的多移动机器人进行实时跟踪已成为研究热点[1-2]。

跟踪过程分为两种基本策略：一种是对机器人建立模型，并通过模型进行算法的应用；另一种则是不建立模型，如文献[3]中提出的直接利用反馈信息对机器人进行控制，但这种控制方式对于需要知道车头方向的车式移动机器人并不适用。

文献[4]建立了非线性的机器人模型，虽然模型的精度较高，但是不适用于计算机控制。

文献[5]中给出的机器人模型适用于轮式机器人，却不适用于车式机器人。

Kalman滤波算法[6]、Condensation算法[7]等数学算法经常用于目标跟踪过程。

由于Condensation算法实时性较差且动态模型精度较低，故很少使用。

Kalman滤波是在已知系统和量测的数学模型、量测噪声统计特性及系统状态初值的情况下，利用输出信号的量测数据和系统模型方程，实时获得系统状态变量和输入信号的最优估计。

Kalman滤波器不仅能对目标轨迹进行滤波和优化，还可以对目标状态进行预估[8]，且实时性较好，适用于多目标跟踪。

卡尔曼滤波器– Kalman Filter1．什么是卡尔曼滤波器（What is the Kalman Filter ）在学习卡尔曼滤波器之前，首先看看为什么叫“卡尔曼”。

跟其他著名的理论（例如傅立叶变换，泰勒级数等等）一样，卡尔曼也是一个人的名字，而跟他们不同的是，他是个现代人！卡尔曼全名Rudolf Emil Kalman，匈牙利数学家，1930年出生于匈牙利首都布达佩斯。

1953，1954年于麻省理工学院分别获得电机工程学士及硕士学位。

1957年于哥伦比亚大学获得博士学位。

我们现在要学习的卡尔曼滤波器，正是源于他的博士论文和1960年发表的论文《A New Approach to Linear Filtering and Prediction Problems》（线性滤波与预测问题的新方法）。

如果对这编论文有兴趣，可以到这里的地址下载：/~welch/media/pdf/Kalman1960.pdf。

简单来说，卡尔曼滤波器是一个“optimal recursive data processing algorithm（最优化自回归数据处理算法）”。

对于解决很大部分的问题，他是最优，效率最高甚至是最有用的。

他的广泛应用已经超过30年，包括机器人导航，控制，传感器数据融合甚至在军事方面的雷达系统以及导弹追踪等等。

近年来更被应用于计算机图像处理，例如头脸识别，图像分割，图像边缘检测等等。

2．卡尔曼滤波器的介绍（Introduction to the Kalman Filter）为了可以更加容易的理解卡尔曼滤波器，这里会应用形象的描述方法来讲解，而不是像大多数参考书那样罗列一大堆的数学公式和数学符号。

但是，他的5条公式是其核心内容。

结合现代的计算机，其实卡尔曼的程序相当的简单，只要你理解了他的那5条公式。

在介绍他的5条公式之前，先让我们来根据下面的例子一步一步的探索。

假设我们要研究的对象是一个房间的温度。