1.引言

当系统状态方程不符合线性假设时，采用卡尔曼滤波无法获得理想的最优估计。高斯分布在非线性系统中的传递结果将不再是高斯分布，参见下图所示。这里x符合高斯分布，y=g(x)为非线性函数，所得结果y的分布已经严重“变形”（这里y的分布通过蒙特卡洛采样获得），统计y的均值和方差也可以画出图中的高斯函数曲线，但已经与实际分布严重不符。

在描述机器人状态时常常不满足卡尔曼滤波的假设,为了仍然能够使用卡尔曼滤波，我们采用对非线性系统进行线性化等方法来扩大卡尔曼滤波的使用范围。扩展卡尔曼滤波与卡尔曼滤波主要区别在于：对状态方程和观测方程泰勒展开。

2. 扩展卡尔曼滤波数学理论

卡尔曼滤波的思想其实很简单，既然系统是非线性的，不是“直线”了，那就用“切线”近似嘛。但是用哪里的切线呢？很明显应该用原分布中可能值最大的那一点处的切线，也就是均值处的切线，如下图所示。所得结果（虚线）与实际结果的均值方差较为接近。

因此我们要将非线性的运动方程和观测方程进行以切线代替的方式来线性化。其实就是在均值处进行一阶泰勒展开。

运动方程线性化
观测方程线性化

该方程近似为自变量 xtx_{t}xt 的线性方程
现在我们只需要用这两个线性化后的方程带入卡尔曼滤波中即可得到扩展卡尔曼滤波。

于是对比卡尔曼滤波，扩展卡尔曼滤波公式为：
由于卡尔曼滤波是基于高斯分布的,所以预测状态量,也就是高斯分布的均值(使得后验概率最大)xt=μtx^{t}=\mu_txt=μt
通过对比EKF和KF的公式会发现，主要区别在于:

对于计算雅克比矩阵，假设状态向量为Xt=[xi,yi,vx,i,vy,i]X_t=[x_i,y_i,v_{x,i},v_{y,i}]Xt=[xi,yi,vx,i,vy,i]，观测向量为 zt=[Ri,θi]bz_t=[R_i,\theta_i]^{b}zt=[Ri,θi]b。根据 zt=h(xt)z_t=h(x_t)zt=h(xt)。

3. 代码实战

3.1 python实现

"""Extended kalman filter (EKF) localization sampleauthor: jiangcheng"""import numpy as np
import math
import matplotlib.pyplot as plt# Estimation parameter of EKF
Q = np.diag([0.1, 0.1, math.radians(1.0), 1.0])**2
R = np.diag([1.0, math.radians(40.0)])**2#  Simulation parameter
Qsim = np.diag([0.5, 0.5])**2
Rsim = np.diag([1.0, math.radians(30.0)])**2DT = 0.1  # time tick [s]
SIM_TIME = 50.0  # simulation time [s]show_animation = Truedef calc_input():v = 1.0  # [m/s]yawrate = 0.1  # [rad/s]u = np.matrix([v, yawrate]).Treturn udef observation(xTrue, xd, u):xTrue = motion_model(xTrue, u)# add noise to gps x-yzx = xTrue[0, 0] + np.random.randn() * Qsim[0, 0]zy = xTrue[1, 0] + np.random.randn() * Qsim[1, 1]z = np.matrix([zx, zy])# add noise to inputud1 = u[0, 0] + np.random.randn() * Rsim[0, 0]ud2 = u[1, 0] + np.random.randn() * Rsim[1, 1]ud = np.matrix([ud1, ud2]).Txd = motion_model(xd, ud)return xTrue, z, xd, uddef motion_model(x, u):F = np.matrix([[1.0, 0, 0, 0],[0, 1.0, 0, 0],[0, 0, 1.0, 0],[0, 0, 0, 0]])B = np.matrix([[DT * math.cos(x[2, 0]), 0],[DT * math.sin(x[2, 0]), 0],[0.0, DT],[1.0, 0.0]])x = F * x + B * ureturn xdef observation_model(x):#  Observation ModelH = np.matrix([[1, 0, 0, 0],[0, 1, 0, 0]])z = H * xreturn zdef jacobF(x, u):"""Jacobian of Motion Modelmotion modelx_{t+1} = x_t+v*dt*cos(yaw)y_{t+1} = y_t+v*dt*sin(yaw)yaw_{t+1} = yaw_t+omega*dtv_{t+1} = v{t}sodx/dyaw = -v*dt*sin(yaw)dx/dv = dt*cos(yaw)dy/dyaw = v*dt*cos(yaw)dy/dv = dt*sin(yaw)"""yaw = x[2, 0]v = u[0, 0]jF = np.matrix([[1.0, 0.0, -DT * v * math.sin(yaw), DT * math.cos(yaw)],[0.0, 1.0, DT * v * math.cos(yaw), DT * math.sin(yaw)],[0.0, 0.0, 1.0, 0.0],[0.0, 0.0, 0.0, 1.0]])return jFdef jacobH(x):# Jacobian of Observation ModeljH = np.matrix([[1, 0, 0, 0],[0, 1, 0, 0]])return jHdef ekf_estimation(xEst, PEst, z, u):#  PredictxPred = motion_model(xEst, u)jF = jacobF(xPred, u)PPred = jF * PEst * jF.T + Q#  UpdatejH = jacobH(xPred)zPred = observation_model(xPred)y = z.T - zPredS = jH * PPred * jH.T + RK = PPred * jH.T * np.linalg.inv(S)xEst = xPred + K * yPEst = (np.eye(len(xEst)) - K * jH) * PPredreturn xEst, PEstdef plot_covariance_ellipse(xEst, PEst):Pxy = PEst[0:2, 0:2]eigval, eigvec = np.linalg.eig(Pxy)if eigval[0] >= eigval[1]:bigind = 0smallind = 1else:bigind = 1smallind = 0t = np.arange(0, 2 * math.pi + 0.1, 0.1)a = math.sqrt(eigval[bigind])b = math.sqrt(eigval[smallind])x = [a * math.cos(it) for it in t]y = [b * math.sin(it) for it in t]angle = math.atan2(eigvec[bigind, 1], eigvec[bigind, 0])R = np.matrix([[math.cos(angle), math.sin(angle)],[-math.sin(angle), math.cos(angle)]])fx = R * np.matrix([x, y])px = np.array(fx[0, :] + xEst[0, 0]).flatten()py = np.array(fx[1, :] + xEst[1, 0]).flatten()plt.plot(px, py, "--r")def main():print(__file__ + " start!!")time = 0.0# State Vector [x y yaw v]'xEst = np.matrix(np.zeros((4, 1)))xTrue = np.matrix(np.zeros((4, 1)))PEst = np.eye(4)xDR = np.matrix(np.zeros((4, 1)))  # Dead reckoning# historyhxEst = xEsthxTrue = xTruehxDR = xTruehz = np.zeros((1, 2))while SIM_TIME >= time:time += DTu = calc_input()xTrue, z, xDR, ud = observation(xTrue, xDR, u)xEst, PEst = ekf_estimation(xEst, PEst, z, ud)# store data historyhxEst = np.hstack((hxEst, xEst))hxDR = np.hstack((hxDR, xDR))hxTrue = np.hstack((hxTrue, xTrue))hz = np.vstack((hz, z))if show_animation:plt.cla()plt.plot(hz[:, 0], hz[:, 1], ".g")plt.plot(np.array(hxTrue[0, :]).flatten(),np.array(hxTrue[1, :]).flatten(), "-b")plt.plot(np.array(hxDR[0, :]).flatten(),np.array(hxDR[1, :]).flatten(), "-k")plt.plot(np.array(hxEst[0, :]).flatten(),np.array(hxEst[1, :]).flatten(), "-r")plot_covariance_ellipse(xEst, PEst)plt.axis("equal")plt.grid(True)plt.pause(0.001)if __name__ == '__main__':main()

3.2结果分析

这是一个传感器融合定位与扩展卡尔曼滤波器(EKF)。
蓝线是正确的轨迹,黑线是航迹推算轨迹,绿点定位观察(GPS),红线与EKF估计轨迹。红色的椭圆与EKF估计协方差椭圆。

无人驾驶算法学习（三）：扩展卡尔曼滤波器Extended Kalman Filter相关推荐

[Math]理解卡尔曼滤波器 (Understanding Kalman Filter)
2. 基本模型 2.1 系统模型卡尔曼滤波模型假设k时刻的真实状态是从(k − 1)时刻的状态演化而来,符合下式: (1) Fk 是作用在 Xk−1 上的状态变换模型(/矩阵/矢量). Bk 是作 ...
【Matlab】扩展卡尔曼滤波器原理及仿真（初学者入门专用）
文章目录 0.引言及友情链接 1.场景预设 2.扩展卡尔曼滤波器 3.仿真及效果 0.引言及友情链接 \qquad卡尔曼滤波器(Kalman Filter, KF)是传感器融合(Sensor Fusi ...
由浅入深的扩展卡尔曼滤波器教程
本篇译文翻译自 The Extended Kalman Filter : An Interactive Turorial for Non-Experts. 原文本文惯例及说明 : 译文中的Demo请 ...
说不尽的卡尔曼 | 详解扩展卡尔曼滤波器
作者 | 火山编辑 | 空中机器人前沿点击下方卡片,关注"自动驾驶之心"公众号 ADAS巨卷干货,即可获取点击进入→自动驾驶之心[目标跟踪]技术交流群后台回复[卡尔曼滤波] ...
【滤波】扩展卡尔曼滤波器（EKF）
%matplotlib inline #format the book import book_format book_format.set_style() 我们发展了线性卡尔曼滤波器的理论.然后在上 ...
【信号处理】基于扩展卡尔曼滤波器和无迹卡尔曼滤波器的窄带信号时变频率估计（Matlab代码实现）
目录 1 概述 2 数学模型 3 运行结果 4 结论 5 参考文献 6 Matlab代码实现 1 概述本文讲解和比较了基于卡尔曼滤波器的频率跟踪方法的能力,例如扩展卡尔曼滤波器 (EKF) 和无味卡 ...
【目标定位】基于matlab扩展卡尔曼滤波器多机器人定位【含Matlab源码 2327期】
⛄一.简介 1 机器人运动模型分析 1.2 坐标系一个是全局坐标系(Xw,Yw), 另一个是机器人的局部坐标系(Xr,Yr). 如下图所示: 图1 机器人坐标系室内环境用二维平面表示在全局坐标系X ...
卡尔曼滤波器、扩展卡尔曼滤波器、无向卡尔曼滤波器的详细推导
这段时间做轴承故障诊断和预测的时候,需要一个针对已经获取了特征向量的工具来对轴承故障状态进行估计和预测.卡尔曼滤波器可以实现对过去.当前和未来目标位置的估计,所以想通过卡尔曼滤波器的设计思路找到一些灵 ...
基于自适应扩展卡尔曼滤波器（AEKF）的锂离子电池SOC估计（附MATLAB代码）
AEKF_SOC_Estimation函数使用二阶RC等效电路模型(ECM)和自适应扩展卡尔曼滤波器(AEKF)估计电池的端电压(Vt)和充电状态(SOC).该函数将以下内容作为输入: · 电流(A) ...

无人驾驶算法学习（三）：扩展卡尔曼滤波器Extended Kalman Filter

文章目录