扩展卡尔曼滤波（EKF）理论讲解与实例（matlab、python和C++代码）
扩展卡尔曼滤波（EKF）理论讲解与实例（matlab、python和C++代码）
⽂章⽬录
我们上篇提到的 （参见我的另⼀篇⽂章： ）是⽤于线性系统，预测（运动）模型和观测模型是在假设⾼斯和线性情况下进⾏的。简单的卡尔曼滤波必须应⽤在符合⾼斯分布的系统中，但是现实中并不是所有的系统都符合这样 。另外⾼斯分布在⾮线性系统中的传递结果将不再是⾼斯分布。那如何解决这个问题呢？扩展卡尔曼滤波就是⼲这个事的。
理论讲解
扩展卡尔曼滤波（Extended Kalman Filter，EKF）通过局部线性来解决⾮线性的问题。将⾮线性的预测⽅程和观测⽅程进⾏求导，以切线代替的⽅式来线性化。其实就是在均值处进⾏⼀阶泰勒展开。
数学中，泰勒公式是⼀个⽤函数在某点的信息描述其附近取值的公式（ ⼀句话描述：就是⽤多项式函数去逼近光滑函数 ）。如果函数⾜够平滑的话，在已知函数在某⼀点的各阶导数值的情况之下，泰勒公式可以⽤这些导数值做系数构建⼀个多项式来近似函数在这⼀点的邻域中的值。泰勒公式还给出了这个多项式和实际的函数值之间的偏差。
表⽰  在第 阶导数的表达式，带⼊⼀个值计算后得到的结果（注意，它是个值）是⼀个系数（⼀个值），每⼀项都不同，第⼀项 ，第⼆项 …… 依此类推
是⼀个以
为⾃变量的表达式 。是泰勒公式的余项，是  的⾼阶⽆穷⼩
KF 和EKF 模型对⽐
⾸先，让卡尔曼先和扩展卡尔曼滤波做⼀个对⽐。在对⽐过程中可以看出，扩展卡尔曼是⼀个简单的⾮线性近似滤波算法，指运动或观测⽅程不是线性的情况，在预测模型部分，扩展卡尔曼的预测模型和量测模型已经是⾮线性了。为了简化计算，EKF 通过⼀阶泰勒分解线性化运动、观测⽅程。KF 与EKF 具有相同的算法结构，都是以⾼斯形式描述后验概率密度的，通过计算贝叶斯递推公式得到的。最⼤的不同之处在于，计算⽅差时，EKF 的状态转移矩阵（上⼀时刻的状态信息）和观测矩阵（⼀步预测）都是状态信息的雅克⽐矩阵（ 偏导数组成的矩阵）。 在预测公式部分，扩展卡尔曼滤波的为的雅可⽐矩阵，在更新公式部分，扩展卡尔曼滤波的为的雅可⽐矩阵。预测模型：
预测公式：
f (x )=Taylor ×n =0∑∞
n !f (a )
(n )(x −a )n
=f (a )+(x −1!f (a )′a )+(x −2!f (a )(2)a )+2⋯+(x −n !f (a )
(n )a )+n R (x )
n f (a )(n )f (x )n a n !11121
(x −a )n x Rn (x )(x −a )n
k −1∣k −1k ∣k −1F k f H k h Kalman Filter
Extended Kalman Filter
x =k Fx +k −1Bu +k w x =k
k f (x ,u )+k −1k w k
=z k Hx +k v =k z k h (x )+k v k
更新公式：
其中为时刻的状态向量；
为状态转移矩阵，表⽰将时刻的状态向量转移⾄t 时刻的状态向量；
是输⼊控制矩阵，代表着将控制向量 映射到状态向量上，统⼀控制向量 和状态向量之间 的关系；代表着控制向量，如加速度，⾓加速度等；
为过程演化噪声；为量测噪声 ；
为状态向量的协⽅差矩阵，代表着状态向量每个元素之间的关系；
表⽰预测状态的⾼斯噪声的协⽅差阵，它⽤来衡量模型的准确度，模型越准确其值越⼩；为传感器测量值的状态向量，也就是传感器的测量结果；
为转换矩阵，他将状态向量 映射到测量值所在的向量空间；为测量值的⾼斯噪声的协⽅差阵，代表着传感器测量的误差。
图⽚⽰例：
Kalman Filter Extended Kalman Filter
=x ^k F +k x ^k −1B =k u k
x ^k f (x ,u )
k −1k P =k F P F +k k −1k T
Q P =k k F P F +k k −1k T
Q k
Kalman Filter
Extended Kalman Filter
K =′P H (H P H +k k T k k k T
R )K =k −1′P H (H P H +k k T k k k T
R )k −1
=x ^k ′+x ^k K (−′z k H )
=k x ^k x ^k ′+x ^k K (−′z k h ())
x ^k P =k ′
P −k K H P P =′k k k ′
P −k K H P ′k k
x k ^k F k k −1B k u
k ^u k ^x k ^u k ^w k v k P k Q k z k H k x
k ^z k R k
均为雅可⽐矩阵。
雅可⽐矩阵计算
J ,J A H
雅可⽐矩阵（Jacobian matrix ）是该函数 的所有⾏数（个）对状态向量的所有分量（ 个）的⼀阶偏导数组成的矩阵。雅可⽐矩阵的计算如下，其中输⼊噪声为0。
函数有 ⾏个分量，于是有⾏。状态向量有个分量，于是有列。雅可⽐矩阵的计算如下，其中输⼊噪声为0。
函数有 ⾏个分量，于是有⾏。状态向量 有个分量，于是有列。
矩阵计算实例
f (x )m x k n F k x =k f (x ,u ，w )≈k −1k k f (,u ,0)+
x ^k −1k x −+
F k −1
∂x
∣,u ,0x
^k −1k (k −1x ^k −1)w L k −1
∂w
∣,u ,0x
^k −1k k
令k =k −1
F k =def ∂x ∂f (x )=def [
∂x 1
∂f ...
如何用matlab将已知点连线∂x n
∂f ]=
def
⎣
⎢⎡∂x 1∂f 1⋮∂x 1
∂f m ...⋮...
∂x n ∂f 1⋮
∂x n
∂f m ⎦⎥⎤=def
J .
A f (x ,u ,w )k −1k k f ,...,f 1m m x k x ,...,x 1n n H k y =k h (x ,v )≈k k h (,0)+
x ^k x −+H k
∂x k ∣,0x ^k (k x ^k )v M k ∂v k ∣,0
x ^k k
H k =def ∂x ∂h (x )=def [
∂x 1
∂h ...
∂x n
∂h ]=
def
⎣
⎢⎡
∂x 1∂h 1⋮∂x 1
∂h m ...⋮...
∂x n ∂h 1⋮
∂x n
∂h m ⎦⎥⎤=def
J .
H h (x ,v )k k h ,...,h 1m m x k x ,...,x 1n n

版权声明：本站内容均来自互联网，仅供演示用，请勿用于商业和其他非法用途。如果侵犯了您的权益请与我们联系QQ:729038198，我们将在24小时内删除。