卡尔曼滤波讲解

EKF算法是一种近似方法，它将非线性模型在状 态估计值附近作泰勒级数展开，并在一阶截断， 用得到的一阶近似项作为原状态方程和测量方 程近似表达形式，从而实现线性化同时假定线 性化后的状态依然服从高斯分布，然后对线性 化后的系统采用标准卡尔曼滤波获得状态估计。 采用局部线性化技术，能得到问题局部最优解， 但它能否收敛于全局最优解，取决于函数的非 线性强度以及展开点的选择。
卡尔曼滤波器的简介
卡尔曼全名Rudolf Emil Kalman，匈牙利数学家， 1930年出生于匈牙利首都布达佩斯。1953， 1954年于麻省理工学院分别获得电机工程学士 及硕士学位。1957年于哥伦比亚大学获得博士 学位。我们现在要学习的卡尔曼滤波器，正是 源于他的博士论文和1960年发表的论文《A New Approach to Linear Filtering and Prediction Problems》（线性滤波与预测问题的新方法）。
y(k)是k时刻的测量值，
H是测量系统的参数，对于多测量系 统，H为矩阵。
系统噪声和测量噪声都是高斯分布的， q(k)和r(k)分别表示过程和测量的噪声。
协方差矩阵分别为Qk-1和Rk
他们被假设成高斯白噪声(White
Gaussian Noise)，他们的covariance分
别是Q，R（这里我们假设他们不随
扩展Kalman滤波算法(EKF)
假定定位跟踪问题的非线性状态方程和测量方程如下:
X f (X ) W ...............(1)
k 1
k
k
Y h(X ) V ...................(.2)
k
k
k
在最近一次状态估计的时刻，对以上两式进行线性化处理，首先构造如 下2个矩阵:
卡尔曼滤波
（kalman filtering)
卡尔曼滤波
一·、卡尔曼滤波器的简介 二、卡尔曼滤波器的应用 三、卡尔曼滤波器（Kalman Filter）基本原理 四、扩展Kalman滤波算法(EKF) 五、无迹卡尔曼滤波算法(UKF) 六、卡尔曼滤波器（Kalman Filter）应用实例（温度）
无迹卡尔曼滤波算法(UKF)
由于近似非线性函数的概率密度分布比近 似非线性函数更容易，使用采样方法近似非线 性分布来解决非线性问题的途径在最近得到了 人们的广泛关注。和EKF一样，UKF也是一种递 归式贝叶斯估计方法，但是UKF不必线性化非 线性状态方程和观测方程，他利用 UT(Unscented transform)方法，用一组确定的采 样点来近似后验概率。
（1）根据输入变量x的统计量x和Px，选择一种sigma点采样策略， 得到输入变量的sigma点集｛xi｝,i=1,…,L,(其中L=2n+1),以及对应 的权值Wmi和Wci。其中：L为所采用的采样策略的采样sigma点 个数， Wmi为均值加权所用权值， Wmi为协方差加权所用权值。 若不采用比例修正，则Wmi= Wci=Wi。
卡尔曼滤波器的简介
卡尔曼滤波的一个典型实例是从一组有限的，包含噪 声的，对物体位置的观察序列（可能有偏差）预测出 物体的位置的坐标及速度。在很多工程应用(如雷达、 计算机视觉)中都可以找到它的身影。同时，卡尔曼滤 波也是控制理论以及控制系统工程中的一个重要课题。 例如,对于雷达来说，人们感兴趣的是其能够跟踪目标。 但目标的位置、速度、加速度的测量值往往在任何时 候都有噪声。卡尔曼滤波利用目标的动态信息，设法 去掉噪声的影响，得到一个关于目标位置的好的估计。 这个估计可以是对当前目标位置的估计(滤波)，也可以 是对于将来位置的估计(预测)，也可以是对过去位置的 估计(插值或平滑)。
F (k 1k) f ( X k ) X
X X (k k)
.......(3)
H (K ) h( X k ) X
X X (k k 1)
...........(4)
扩展Kalman滤波算法(EKF)
将线性化后的状态转移矩阵和观测矩阵代入到标准卡尔曼滤波框 架中，即得到扩展卡尔曼滤波。
因为EKF忽略了非线性函数泰勒展开的高阶项，仅仅用了一阶项， 是非线性函数在局部线性化的结果，这就给估计带来了很大误差， 所以只有当系统的状态方程和观测方程都接近线性且连续时，EKF 的滤波结果才有可能接近真实值。EKF滤波结果的好坏还与状态噪 声和观测噪声的统计特性有关，在EKF的递推滤波过程中，状态噪 声和观测噪声的协方差矩阵保持不变，如果这两个噪声协方差矩 阵估计的不够准确，那就容易产生误差累计，导致滤波器发散。 EKF的另外一个缺点是初始状态不太好确定，如果假设的初始状态 和初始协方差误差较大，也容易导致滤波器发散。
卡尔曼滤波器的应用
卡尔曼滤波器是一个“optimal recursive data processing algorithm（最优化自回归数据处理算法）”。对于解 决很大部分的问题，他是最优，效率最高甚至是最有 用的。
他的广泛应用已经超过30年，包括机器人导航，控制， 传感器数据融合甚至在军事方面的雷达系统以及导弹 追踪等等。近年来更被应用于计算机图像处理，例如 头脸识别，图像分割，图像边缘检测等等。
系统状态变化而变化）。
卡尔曼滤波器——时间更新和状态更新
时间更新
xˆ xˆ u A B
k
k 1
k 1
P p A A
T Q
k
k 1
状态更新
K P H P H
k
k
TH k
ቤተ መጻሕፍቲ ባይዱ
T R 1
xˆk xˆk K k
y k
H
xˆk
Pk
1
K
k
H
P
k
扩展Kalman滤波算法(EKF)
无迹卡尔曼滤波算法(UKF)——UT变换
UT变换是UKF算法的核心和基础。UT变换的思想是： 在确保采样均值和协方差为X和Px的前提下，选择一组 点集（sigma点集），将非线性变换应用于采样的每个 sigma点，得到非线性转换后的点集y和Py是变换后 sigma点集的统计量。
无迹卡尔曼滤波算法(UKF)——UT变换
卡尔曼滤波器（Kalman Filter）基本原理
基本假设：
x(k)是k时刻的系统状态
后验概率分布p(xk-1/yk-1) 为高斯分布 动态系统是线性的
x x u q A B
k
k 1
k 1 k 1
y k
H
xk
rk
u(k)是k时刻对系统的控制量。
A和B是系统参数，对于多模型系统， 他们为矩阵。