非线性卡尔曼滤波EKF与UKF
卡尔曼滤波器分类及基本公式
以 时刻的最优估计 为准，预测 时刻的状态变量 ，同时又对该状态进行观测，得到观测变量 ，再在预测与观测之间进行分析，或者说是以观测量对预测量进行修正，从而得到 时刻的最优状态估计 。
卡尔曼滤波思想
卡尔曼滤波的由来
卡尔曼滤波的由来
卡尔曼滤波理论作为最优估计的一种，它的创立是科学技术和社会需要发展到一定程度的必然结果。在1795年，高斯为测定行星运动轨道而提出最小二乘估计法。为了解决火力控制系统精度跟踪问题，维纳于1942年提出了维纳滤波理论，利用有用信号和干扰信号的功率谱确定线性滤波器的频率特性，首次将数理统计理论与线性理论有机的联系在一起，形成了对随机信号做平滑、估计或者预测的最优估计新理论。但是采用频域设计法是造成维纳滤波器设计困难的根本原因。于是，人们逐渐转向寻求在时域内直接设计最优滤波器的方法，而卡尔曼研究的卡尔曼滤波理论很好的解决了这个问题
卡尔曼滤波的基本方程
例子
卡尔曼滤波的基本方程
现在，我们用于估算K时刻房间的实际温度有两个温度值：估计值23度和测量值25度。究竟实际温度是多少呢？是相信自己还是相信温度计？究竟相信谁多一点？我们需要用他们的均方误差来判断。因为， （*公式三），所以我们可以估算出K时刻的最优温度值为： 度（*公式四）。 得到了K时刻的最优温度，下一步就是对K+1时刻的温度值进行最优估算，需要得到K时刻的最优温度（24.56）的偏差，算法如下： （*公式五） 就这样，卡尔曼滤波器就不断的把均方误差递归，从而估算出最优的温度值，运行速度快，且只保留上一时刻的协方差。
卡尔曼滤波的由来
卡尔曼，全名Rudolf Emil Kalman，匈牙利数学家，1930年出生于匈牙利首都布达佩斯。1953，1954年于麻省理工学院分别获得电机工程学士及硕士学位。1957年于哥伦比亚大学获得博士学位。我们在现代控制理论中要学习的卡尔曼滤波器，正是源于他的博士论文和1960年发表的论文《A New Approach to Linear Filtering and Prediction Problems》（线性滤波与预测问题的新方法）。
非线性卡尔曼滤波器
UKF计算步骤：
PF
PF
● 粒子滤波(PF: Particle Filter)的思想基于蒙特卡洛方法(Monte Carlo methods)，它是 利用粒子集来表示概率，可以用在任何形式的状态空间模型上。其核心思想是通过从后 验概率中抽取的随机状态粒子来表达其分布，是一种顺序重要性采样法(Sequential Importance Sampling)。简单来说，粒子滤波法是指通过寻找一组在状态空间传播的随 机样本对概率密度函数进行近似，以样本均值代替积分运算，从而获得状态最小方差分 布的过程。这里的样本即指粒子,当样本数量N→∝时可以逼近任何形式的概率密度分布。
EKF
首先围绕滤波值 xˆk 将非线性函数 f , g 展开Taylor级数并
略去二阶及以上项，得到一个近似的线性化模型，然后应用 Kalman滤波完成对其目标的滤波估计等处理。
1.对状态模型的一阶Taylor展示：
xk f
xˆk 1
f xˆk 1
xk 1 xˆk 1 k
令
f
F
xˆk 1
f1 f1
xˆ1
xˆ2
f xˆk 1
F
f2 xˆ1
f2 xˆ2
fn fn
xˆ1 xˆ2
f1
xˆn
f2 xˆn
fn
xˆn
g1 g1
xˆ1
xˆ2
g xˆk
H
g2 xˆ1
g2 xˆ2
扩展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仍是用高斯分布来逼近系统状态的后验概 率密度，所以在系统状态的后验概率密度是非高斯 的情况下，滤波结果将有极大的误差。
EKF和UKF
卡尔曼滤波器估计一个用线性随机差分方程描述 的离散时间过程的状态变量。 但如果被估计的过程和（或）观测变量与过程的 关系是非线性的，那应称作扩展卡 尔曼滤波器（Extended Kalman Filter），简称 EKF。
非线性系统模型为：
状态初始条件为：
时间更新
测量更新
UKF通过引入确定样本的方法,用较少的样本点 来表示状态的分布,这些样本点能够准确地捕获 高斯随机变量的均值和协方差矩阵,当其通过任 意非线性函数时,函数输出值能够拟合真实函数 值,精度可以逼近3阶以上。
EKF只能达到一阶,而且需要计算雅可比矩阵。 UKF只需计算几次几个伪状态点的预测状态值, 计算复杂度稍小于EKF,但精度与稳定性远高于 EKF,从而可以大大改善滤波效果。
我们将卡尔曼滤波的公式换一种方式表示， 使其状态方程变为非线性随机差分方程的形式： 观测方程为：
随机变量wk和vk仍代表过程激励噪声和观测噪 声。 状态方程中的非线性函数f将上一时刻k-1的状态 映射到当前时刻k的状态。 k 观测方程中的驱动函数uk和零均值过程噪声wk 是它的参数。非线性函数h反映了状态变量xk和 观测变量zk的关系。
实际中我们显然不知道每一时刻噪声wk和vk各 自的值。 我们可以将它们假设为零，从而估计状态向量和 观测向量为： 和 其中，xk是过程相对前一时刻k的后验估计。
以下给出了扩展卡尔曼滤波器的全部表达式。 注意我们用 替换 了来表达先验概率的 意思，并且雅可比矩阵A,W,H,V也被加上了下标， 明了它们在不同的时刻具有变化的值，每次需要 被重复计算。
扩展卡尔曼滤波器时间更新方程
Ak和Wk是k时刻的过程雅可比矩阵 Qk是中k时刻的过程激励噪声协方差矩阵。
EKFUKFPF算法的比较程序
EKFUKFPF算法的比较程序在估计理论中，EKF（Extended Kalman Filter），UKF（Unscented Kalman Filter）和PF（Particle Filter）是三种常用的非线性滤波算法。
它们在不同的环境和应用中具有不同的优点和缺点。
下面将对这三种算法进行比较。
首先，EKF是最常用的非线性滤波算法之一、它通过线性化状态转移方程和测量方程来近似非线性问题。
EKF在处理高斯噪声的情况下表现良好，但在处理非高斯噪声时会有较大的误差。
由于线性化过程的存在，EKF对于高度非线性和非高斯问题可能表现不佳。
此外，EKF对系统模型的准确性要求较高，较大的模型误差可能导致滤波结果的不准确性。
其次，UKF通过构造一组代表系统状态的Sigma点，通过非线性映射来近似非线性函数。
相较于EKF，UKF无需线性化系统模型，因此适用于更广泛的非线性系统。
UKF的优点是相对较好地处理了非线性系统和非高斯噪声，但在处理维数较高的问题时，计算开销较大。
最后，PF是一种基于粒子的滤波方法，通过使用一组代表系统状态的粒子来近似概率密度函数。
PF的优点是它可以处理非线性系统和非高斯噪声，并且在系统模型不准确或缺乏确定性时，具有较好的鲁棒性。
由于粒子的数量可以灵活调整，PF可以提供较高的估计精度。
然而，PF的计算开销较大，尤其在高维度的情况下。
综上所述，EKF、UKF和PF是三种常用的非线性滤波算法。
EKF适用于高斯噪声条件下的非线性问题，但对系统模型准确性要求高。
UKF适用于一般的非线性问题，但计算开销较大。
PF适用于非线性和非高斯噪声条件下的问题，并具有较好的鲁棒性，但在计算开销方面具有一定的挑战。
在实际应用中，我们应根据具体问题的性质和要求选择合适的算法。
比如，在低维情况下，EKF是一个可行的选择；在高维或非高斯噪声情况下，可以考虑使用UKF或PF算法。
ukf滤波算法范文
ukf滤波算法范文UKF（Unscented Kalman Filter）是一种基于卡尔曼滤波的非线性滤波算法。
相比于传统的扩展卡尔曼滤波（EKF），UKF通过一种更好的方法来近似非线性系统的概率分布，从而提高了非线性滤波的精确度和鲁棒性。
UKF通过一种称为“无气味变换（unscented transform）”的方法来处理非线性函数。
该方法基于对概率分布的均值和协方差进行一系列的采样点选择，然后通过变换这些采样点来近似非线性函数的传播。
这些采样点被称为“Sigma点”，可以看作是真实系统状态在均值周围的一系列假设状态。
UKF的基本步骤如下：1.初始化：初始化系统状态和协方差矩阵。
2. 预测步骤（Prediction）：- 通过生成Sigma点来近似系统状态的概率分布。
- 将Sigma点通过非线性函数进行变换，得到预测状态和预测协方差矩阵。
-计算预测状态的均值和协方差。
3. 更新步骤（Update）：- 通过生成Sigma点来近似测量函数的概率分布。
- 将预测状态的Sigma点通过测量函数进行变换，得到预测测量和预测测量协方差矩阵。
-计算预测测量的均值和协方差。
-根据实际测量值和预测测量的概率分布，计算卡尔曼增益。
-更新预测状态和协方差。
UKF相比于EKF具有以下优势：1.不需要对非线性函数进行线性化。
EKF通过一阶泰勒展开来线性化非线性函数，这可能导致误差积累和不稳定性。
UKF通过采样点直接逼近非线性函数，避免了这个问题。
2.更好的估计准确度和收敛性。
UKF通过采样点的选择更好地逼近了真实概率分布，提高了滤波的准确度和收敛性。
3.适用于高维状态空间。
EKF在高维状态空间中存在计算复杂度高和数值不稳定的问题，而UKF则通过更好的采样点选择来解决了这个问题。
4.对初始条件不敏感。
UKF对初始条件的选择不太敏感，可以在一定程度上避免初始条件选择不当导致的滤波失效问题。
尽管UKF相比于EKF有许多优势，但它也存在一些缺点。
EKF与UKF的性能比较及应用
EKF与UKF的性能比较及应用张文;孙瑞胜【期刊名称】《南京理工大学学报（自然科学版）》【年(卷),期】2015(000)005【摘要】该文分析了扩展卡尔曼滤波(EKF)、无迹卡尔曼滤波(UKF)的基本原理和特点,并以跟踪系统和捷联惯导系统( SINS)的动基座对准为对象,分别采用这两种算法对其进行了仿真。
仿真结果表明,UKF的滤波精度高于EKF,说明对于强非线性系统,UKF比EKF具有更强的优越性,可以广泛应用在目标跟踪系统和航天航空领域。
%This paper especially analyzes the basic principles and characteristics of extended Kalman filter(EKF)and unscented Kalman filter(UKF). Simulations are made in one-dimensional nonlinear tracking system and the moving base of strapedown inertial navigation systems(SINS)alignment. The results demonstrate that the precision of UKF is higher than that of EKF for the nonlinear system,and it can be widely used for the target tracking and the aerospace.【总页数】5页(P614-618)【作者】张文;孙瑞胜【作者单位】南京理工大学泰州科技学院，江苏泰州225300;南京理工大学能源与动力工程学院，江苏南京210094【正文语种】中文【中图分类】TP212【相关文献】1.利用EKF、CMKF和UKF算法处理模型非线性的性能比较 [J], 廖咏一;岳小飞;郭丽宝2.EKF和SUKF在大摇摆基座初始对准中的应用 [J], 张金亮;秦永元;梅春波3.EKF与UKF的目标跟踪算法应用与对比 [J], 唐哲;王庭军;陈志豪4.EKF和UKF在角度测量定位技术中的应用与分析 [J], 孙冬5.EKF和UKF算法在无线传感器网络目标跟踪中的应用 [J], 史岩;朱涛;傅军;张亚宁因版权原因，仅展示原文概要，查看原文内容请购买。
利用EKF、CMKF和UKF算法处理模型非线性的性能比较-精品文档
利用EKF、CMKF和UKF算法处理模型非线性的性能比较1 概述地空导弹武器系统的制导雷达对空中目标进行探测跟踪，可独立测得极坐标下的目标斜距、高低角和方位角；目标运动的状态可由位置、速度和加速度三变量在直角坐标系中线性地表示。
由于极坐标与直角坐标系之间存在非线性的变换关系，使得在对运动目标的跟踪中，目标的运动状态方程和量测方程存在非线性方程。
通常为了更直观和方便的描述目标运动过程，将目标运动状态方程选在直角坐标系中建立，则量测方程为非线性形式，对非线性量测方程的处理方式影响了滤波器的跟踪特性。
本文主要针对目前EKF、CMKF和UKF三种对非线性问题的处理方法从滤波性能、滤波系统跟踪特性以及计算量等方面进行对比分析，利于滤波算法的合理选择，改善地空导弹武器系统对目标的探测跟踪性能。
2 直角坐标系下的目标运动模型由于地空导弹武器系统所拦截的空中目标存在转弯、逃避等多种机动飞行情况，为更为真实的反映目标机动范围和强度的变化，采用非零均值和修正瑞利分布表征机动加速度特性，因此，建立在直角坐标系下的基于机动目标“当前”统计模型的目标的状态方程。
3 量测方程非线性的处理方法（1）EKF滤波算法（2）CMKF滤波算法（3）UKF滤波算法UKF滤波算法对卡尔曼滤波中的一步预测方程，使用无迹（UT）变换来处理均值和协方差的非线性传递。
UT变换的基本思路为：将被估计状态视为随机变量，利用采样策略，在原先状态分布中选取一些点，使其均值和协方差与状态分布的均值和协方差相等，将这些点代入非线性函数中，得到非线性函数值点集，然后在测量的基础上调节样本点的位置，使得样本均值和样本方差分别以二次精度逼近实际分布的均值和方差，得到非线性系统方程和量测方程的均值和方差后，利用卡尔曼线性滤波框架中的测量更新，可获得状态变量的估计值。
4 仿真比较针对目标飞行速度600m/s、飞行高度6km、初始斜距21km，目标水平蛇形机动飞行；制导雷达测量的目标斜距的量测噪声服从（0，25m）的高斯分布，高低和方位角度噪声服从（0，0.2mrad）高斯分布，目标运动模型中的系统方差Q和量测方差R值相同，对比采用EKF、CMKF以及UKF对量测方差非线性处理下的性能，如下图1至图5所示。
- 1、下载文档前请自行甄别文档内容的完整性，平台不提供额外的编辑、内容补充、找答案等附加服务。
- 2、"仅部分预览"的文档,不可在线预览部分如存在完整性等问题,可反馈申请退款(可完整预览的文档不适用该条件!)。
- 3、如文档侵犯您的权益，请联系客服反馈,我们会尽快为您处理(人工客服工作时间：9:00-18:30)。
三、无损卡尔曼滤波器
非线性系统模型为：
Y (k ) h( X (k )) (k ) X (k ) f ( X (k 1)) (k 1)
状态初始条件为：
ˆ (0 | 0) E[ X (0 | 0)] X ˆ (0 | 0)( X (0 | 0) X ˆ (0 | 0)T ] PXX (0 | 0) E[ X (0 | 0) X
三、无损卡尔曼滤波器
• UKF（Unscented Kalman Filter)，中文
释义是无损卡尔曼滤波、无迹卡尔曼滤波或 者去芳香卡尔曼滤波。 • 是无损变换(UT) 和标准Kalman滤波体系 的结合，通过无损变换使非线性系统方程适
用于线性假设下的标准Kalman滤波体系。
三、无损卡尔曼滤波器
谢谢！
时就需要采用非线性卡尔曼滤波技术。 • 目前，应用最为广泛的非线性卡尔曼滤波方法 是将非线性方程近似成线性方程，忽略非线性 部分，得到次优状态估计的非线性滤波。 • 扩展卡尔曼滤波器和无迹卡尔曼滤波器
二、扩展卡尔曼滤波器
• 将期望和方差线性化的卡尔曼滤波器称
作 扩 展 卡 尔 曼 滤 波 器 （ Extended Kalman Filter），简称EKF。
三、无损卡尔曼滤波器
计算sigma点
^ k 1 x k 1 x k 1 Pk 1
^
x k 1 Pk 1
^
时间更新
(k | k 1) f ( (k 1| k 1), k 1)
ˆ (k | k 1) W m (k | k 1) X i i
k
xk 是过程相对前一时刻k的后验源自计。 其中，二、扩展卡尔曼滤波器
以下给出了扩展卡尔曼滤波器的全部表
达式。 注意我们用 x k 替换 xk 了来表达先验概率 的意思，并且雅可比矩阵A,W,H,V也被加上 了下标，明了它们在不同的时刻具有变化的
值，每次需要被重复计算。
二、扩展卡尔曼滤波器
扩展卡尔曼滤波器时间更新方程
k T k
H P H
k k
T k
x k x K k ( zk h x , 0 )
时刻的观测噪声协方差矩阵（注意下标k 表示 Rk 随时间变化）。
P k I Kk H k P H k 和V是k时刻的测量雅可比矩阵， Rk 是k
k
k
Vk RkV
T k
二、扩展卡尔曼滤波器
我们将卡尔曼滤波的公式换一种方式 表示，使其状态方程变为非线性随机差 分方程的形式：
xk f xk 1,uk 1, wk 1
观测方程为：
zk h xk , uk
二、扩展卡尔曼滤波器
• 随机变量 wk 和vk 仍代表过程激励噪声和观
测噪声。 • 状态方程中的非线性函数f将上一时刻k-1 的状态映射到当前时刻k的状态。 • 观测方程中的驱动函数 uk和零均值过程噪
非线性卡尔曼滤波器
——EKF与UKF
目录
前言
扩展 卡尔曼滤波
无损 卡尔曼滤波
Matlab仿真
一、前言
• 卡尔曼滤波（Kalman filtering）一种利用线性 系统状态方程，通过系统输入输出观测数据， 对系统状态进行最优估计的算法。 • 适用于线性、离散和有限维系统。
一、前言
• 实际应用中，非线性的现象是十分普遍的，此
T PXX (k | k ) PXX (k | k 1) K (k )P ( k | k 1) K (k ) YY
四、Matlab仿真
系统方程：
25 x k 1 x k 0.5 x k 1 8cos 1.2 k 1 Q W 2 1 x k 1 x2 k Y k R V 20
状态
100 80 60 40 20 0
0
50 时间步长
100
150
四、Matlab仿真
由仿真结果可知，UKF有着更高的精度。
UKF是对非线性函数的概率密度分布进行近似，用
一系列确定样本来逼近状态的后验概率密度，而不 是对非线性函数进行近似，不需要求导计算 Jacobian矩阵。 UKF没有线性化忽略高阶项，因此非线性分布统计 量的计算精度较高。
i 0 2L
(k | k 1) h( (k | k 1), k 1)
ˆ (k | k 1) W m (k | k 1) Y i i
i 0 2L
三、无损卡尔曼滤波器
测量更新
ˆ ( k | k 1)) (Y ( k | k 1) Y ˆ( k | k 1)) T ] PXY (k | k 1) Wi c [( i (k | k 1) X i
1
二、扩展卡尔曼滤波器
EKF应用于非线性状态估计系统中已经得到了
学术界认可并为人广泛使用，但这种方法存在 两个缺点： • 当强非线性时，Taylor展开式中被忽略的高 阶项带来大的误差，EKF算法可能发散；
• 由于EKF在线性化处理时需要用雅克比矩
阵，其繁琐的计算过程导致该方法实现相 对困难。
其中，Q=4，R=1，
W和N分别为互不相关的高斯白噪声
四、Matlab仿真
200 真实值 EKF估 计 值 UKF估 计 值
150
100
状态
50 0 -50 0
50 时间步长
100
150
四、Matlab仿真
200 180 160 140 120 EKF估 计 值 误 差 UKF估 计 值 误 差
i 0 2L
ˆ (k | k 1) Y (k | k 1) Y ˆ (k | k 1) R PYY k | k 1 Wi Y ( k | k 1) Y i i i 0
c
2L
T
K (k ) PXX (k | k 1) P 1YY (k | k 1) ˆ (k | k ) X ˆ (k | k 1) K (k )(Y (k ) Y ˆ (k | k 1)) X
UKF通过引入确定样本的方法,用较少的样本
点来表示状态的分布,这些样本点能够准确地
捕获高斯随机变量的均值和协方差矩阵,当其
通过任意非线性函数时,函数输出值能够拟合
真实函数值,精度可以逼近3阶以上。
三、无损卡尔曼滤波器
UKF=Unscented Transform + Kalman Filter
即UKF可以看成是基于UT技术的卡尔曼滤波器。 在卡尔曼滤波算法中，对于一步预测方程，使 用UT变换来处理均值和协方差的非线性传递。
xk f xk 1 , uk 1 ,0
T T P A P A Q k k k 1 k k 1 k 1 k 1
Ak 是k时刻的过程雅可比矩阵 Qk 是中k时刻的过程激励噪声协方差矩阵。
二、扩展卡尔曼滤波器
• 扩展卡尔曼滤波器状态更新方程
Kk P H
k
声 wk 是它的参数。非线性函数h反映了状
态变量 xk和观测变量 zk 的关系。
二、扩展卡尔曼滤波器
实际中我们显然不知道每一时刻噪声wk 和 vk 各自的值。 我们可以将它们假设为零，从而估计状态向 量和观测向量为：
x k f x k 1 , uk 1 , 0
和
zk
h x ,0