《扩展Kalman滤波(EKF)和无迹卡尔曼滤波ppt课件》由会员分享，可在线阅读，更多相关《扩展Kalman滤波(EKF)和无迹卡尔曼滤波ppt课件（29页珍藏版）》请在金锄头文库上搜索。
1、 EKF与UKF 一 背景 普通卡尔曼滤波是在线性高斯情况下利用最小均方误差准则获得目标的动态估计 适应于过程和测量都属于线性系统 且误差符合高斯分布的系统 但是实际上很多系统都存在一定的非线性 表现在过程方程 状态方程 是非线性的 或者观测与状态之间的关系 测量方程 是非线性的 这种情况下就不能使用一般的卡尔曼滤波了 解决的方法是将非线性关系进行线性近似 将其转化成线性问题 对于非线性问题线性化常用的两大途径 1 将非线性环节线性化 对高阶项采用忽略或逼近措施 EKF 2 用采样方法近似非线性分布 UKF 二 扩展Kalman滤波 EKF 算法 EKF算法是一种近似方法 它将非线性模型在状态
2、估计值附近作泰勒级数展开 并在一阶截断 用得到的一阶近似项作为原状态方程和测量方程近似表达形式 从而实现线性化同时假定线性化后的状态依然服从高斯分布 然后对线性化后的系统采用标准卡尔曼滤波获得状态估计 采用局部线性化技术 能得到问题局部最优解 但它能否收敛于全局最优解 取决于函数的非线性强度以及展开点的选择 二 扩展Kalman滤波 EKF 算法 假定定位跟踪问题的非线性状态方程和测量方程如下 在最近一次状态估计的时刻 对以上两式进行线性化处理 首先构造如下2个矩阵 二 扩展Kalman滤波 EKF 算法 将线性化后的状态转移矩阵和观测矩阵代入到标准卡尔曼滤波框架中 即得到扩展卡尔曼滤波 因为
3、EKF忽略了非线性函数泰勒展开的高阶项 仅仅用了一阶项 是非线性函数在局部线性化的结果 这就给估计带来了很大误差 所以只有当系统的状态方程和观测方程都接近线性且连续时 EKF的滤波结果才有可能接近真实值 EKF滤波结果的好坏还与状态噪声和观测噪声的统计特性有关 在EKF的递推滤波过程中 状态噪声和观测噪声的协方差矩阵保持不变 如果这两个噪声协方差矩阵估计的不够准确 那就容易产生误差累计 导致滤波器发散 EKF的另外一个缺点是初始状态不太好确定 如果假设的初始状态和初始协方差误差较大 也容易导致滤波器发散 二 扩展Kalman滤波 EKF 算法 Matlab程序 functiontest ekf
4、kx 01 ky 05 阻尼系数g 9 8 重力t 10 仿真时间Ts 0 1 采样周期len fix t Ts 仿真步数 真实轨迹模拟dax 1 5 day 1 5 系统噪声X zeros len 4 X 1 0 50 500 0 状态模拟的初值fork 2 lenx X k 1 1 vx X k 1 2 y X k 1 3 vy X k 1 4 x x vx Ts vx vx kx vx 2 dax randn 1 1 Ts y y vy Ts 二 扩展Kalman滤波 EKF 算法 vy vy ky vy 2 g day randn 1 Ts X k x vx y vy endfigur
5、e 1 holdoff plot X 1 X 3 b gridon figure 2 plot X 2 2 4 构造量测量mrad 0 001 dr 10 dafa 10 mrad 量测噪声fork 1 lenr sqrt X k 1 2 X k 3 2 dr randn 1 1 a atan X k 1 X k 3 dafa randn 1 1 Z k r a end 二 扩展Kalman滤波 EKF 算法 figure 1 holdon plot Z 1 sin Z 2 Z 1 cos Z 2 ekf滤波Qk diag 0 dax 0 day 2 Rk diag dr dafa 2 Xk
6、zeros 4 1 Pk 100 eye 4 X est X fork 1 lenFt JacobianF X k kx ky g Hk JacobianH X k fX fff X k kx ky g Ts hfX hhh fX Ts Xk Pk Kk ekf eye 4 Ft Ts Qk fX Pk Hk Rk Z k hfX X est k Xk end 二 扩展Kalman滤波 EKF 算法 figure 1 plot X est 1 X est 3 r xlabel X ylabel Y title ekfsimulation legend real measurement ekfe
7、stimated 子程序 functionF JacobianF X kx ky g 系统状态雅可比函数vx X 2 vy X 4 F zeros 4 4 F 1 F 2 2 2 kx vx F 3 4 1 F 4 4 2 ky vy 2 1 二 扩展Kalman滤波 EKF 算法 functionH JacobianH X 量测雅可比函数x X 1 y X 3 H zeros 2 4 r sqrt x 2 y 2 H 1 1 1 r H 1 3 1 r xy2 1 x y 2 H 2 1 1 xy2 1 y H 2 3 1 xy2 x 1 y 2 functionfX fff X kx ky
8、 g Ts 系统状态非线性函数x X 1 vx X 2 y X 3 vy X 4 x1 x vx Ts vx1 vx kx vx 2 Ts y1 y vy Ts vy1 vy ky vy 2 g Ts fX x1 vx1 y1 vy1 二 扩展Kalman滤波 EKF 算法 functionhfX hhh fX Ts 量测非线性函数x fX 1 y fX 3 r sqrt x 2 y 2 a atan x y hfX r a function Xk Pk Kk ekf Phikk 1 Qk fXk 1 Pk 1 Hk Rk Zk hfX ekf滤波函数Pkk 1 Phikk 1 Pk 1 Ph
9、ikk 1 Qk Pxz Pkk 1 Hk Pzz Hk Pxz Rk Kk Pxz Pzz 1 Xk fXk 1 Kk Zk hfX Pk Pkk 1 Kk Pzz Kk 二 扩展Kalman滤波 EKF 算法 图2仿真结果 三 无迹卡尔曼滤波算法 UKF 为了改善对非线性问题进行滤波的效果 Julier等人提出了采用基于unscented变换的UKF方法UKF不是和EKF一样去近似非线性模型 而是对后验概率密度进行近似来得到次优的滤波算法 UKF算法的核心是UT变换 UT是一种计算非线性变换中的随机变量的统计特征的新方法 是UKF的基础 三 无迹卡尔曼滤波算法 UKF 假设n维随机向量 x
10、通过非线性函数y f x 变换后得到n维的随机变量y 通过UT变换可以以较高的精度和较低的计算复杂度求得y的均值和方差 UT的具体过程可描述如下 1 计算2n 1个Sigma点及其权值 三 无迹卡尔曼滤波算法 UKF 式中 决定Sigma点的散布程度 通常取一小的正值 k通常取0 用来描述x的分布信息了高斯情况下 的最优值为2 为矩阵平方根第i列 为均值的权值 为方差的权值 2 计算Sigma点通过非线性函数的传播结果 从而可知 三 无迹卡尔曼滤波算法 UKF 由于x的均值和方差都精确到二阶 计算得到y的均值和方差也精确到二阶 比线性化模型精度更高 在卡尔曼框架内应用UT技术就得到了UKF算法
11、 三 无迹卡尔曼滤波算法 UKF UKF是用确定的采样来近似状态的后验PDF 可以有效解决由系统非线性的加剧而引起的滤波发散问题 但UKF仍是用高斯分布来逼近系统状态的后验概率密度 所以在系统状态的后验概率密度是非高斯的情况下 滤波结果将有极大的误差 三 无迹卡尔曼滤波算法 UKF Matlab程序 functionUKFmain 清屏 closeall clearall clc tic globalQfn 定义全局变量 初始化 stater0 220 1 55 0 5 标准系统初值state0 200 1 3 50 0 3 测量状态初值 系统滤波初始化p 0 005000 00 00500
12、000 0050 0000 005 状态误差协方差初值n 4 T 3 Qf T 2 20 0T T 2 20 0T stater stater0 state state0 xc state staterout stateout xcout errorout tout t0 1 h 1 tf 1000 仿真时间设置 三 无迹卡尔曼滤波算法 UKF 滤波算法 fort t0 h tf state stater yc track state stater 轨迹发生器 标准轨迹和输出 xc p UKFfiter systemfun measurefun xc yc p error xc stater
13、滤波处理后的误差staterout staterout stater stateout stateout state errorout errorout error xcout xcout xc tout tout t end 状态信息图像 figure plot tout xcout 1 r tout staterout 1 g tout stateout 1 black legend 滤波后 真实值 无滤波 gridon xlabel 时间t s ylabel 系统状态A title 无迹卡尔曼滤波 figure 三 无迹卡尔曼滤波算法 UKF plot tout xcout 2 r to
14、ut staterout 2 g tout stateout 2 black gridon legend 滤波后 真实值 无滤波 xlabel 时间t s ylabel 系统状态B title 无迹卡尔曼滤波 figure plot tout xcout 3 r tout staterout 3 g tout stateout 3 black gridon legend 滤波后 真实值 无滤波 xlabel 时间t s ylabel 系统状态C title 无迹卡尔曼滤波 figure plot tout xcout 4 r tout staterout 4 g tout stateout 4
15、 black 三 无迹卡尔曼滤波算法 UKF gridon legend 滤波后 真实值 无滤波 xlabel 时间t s ylabel 系统状态D title 无迹卡尔曼滤波 figure plot tout errorout 1 r tout errorout 2 g tout errorout 3 black tout errorout 4 b gridon legend A B C D xlabel 时间t s ylabel 滤波后的状态误差 title 无迹卡尔曼滤波误差 toc 计算仿真程序运行时间end 三 无迹卡尔曼滤波算法 UKF function state stater
16、yout track state0 stater0 轨迹发生函数 T 3 F 1T00 0100 001T 0001 G T 2 20 0T T 2 20 0T V 0 005 randn 2 1 W 0 008 randn 1 1 state F state0 G V stater F stater0 yout atan stater0 3 stater0 1 W 用真实值得到测量值 在滤波时结果才会与真实值重合 endfunctionstate systemfun state0 系统方程 T 3 F 1T00 0100 001T 0001 state F state0 end 三 无迹卡尔曼滤波算法 UKF functionyout measurefun state0 测量方程 yout atan state0 3 state0 1 endfunction xc p UKFfiter systemfun measurefun xc0 yc p0 此程序用于描述UKF 无迹kalman滤波 算法 globalQfn 参数注解 xc0 状态初值 列向量 yc 系统测量值 p0 状态误差
