UKF算法滤波性能分析

精品文档 1欢迎下载1欢迎下载1欢迎下载 UKF 算法滤波性能分析 高海南高海南 31100380113110038011 一 仿真问题描述一 仿真问题描述 考虑一个在二维平面 x y 内运动的质点 M 其在某一时刻 k 的位置 速度 和加速度可用矢量表示 假设 M 在水平方向 x 作近 T kkkkkk kxyxyxy x 似匀加速直线运动 垂直方向 y 上亦作近似匀加速直线运动 两方向上运动 具有加性系统噪声 则在笛卡尔坐标系下该质点的运动状态方程为 kw 1 kk kkkkk xfxwF xw 其中 2 2 1000 2 0100 2 00100 00010 000010 000001 k t t t t t t F 假设一坐标位置为 0 0 的雷达对 M 进行测距和测角 实际测量中 k r k 雷达具有加性测量噪声 则在传感器极坐标系下 观测方程为 kv 22 1 tan kkr kr k k k k xyv k rv k kkk y vk vk x zhxv 显然在笛卡尔坐标系下 该模型运动观测方程为非线性的 我们根据雷达 测量值使用 UKF 算法对目标进行跟踪 并与 EKF 算法结果进行比较 精品文档 2欢迎下载2欢迎下载2欢迎下载 二 问题分析二 问题分析 1 UKF 滤波跟踪 对于非线性系统 设具有协方差阵 1 k k kkk kkk xf xw zhxv kw k Q 具有协方差阵 ukf 算法步骤如下 kv k R 1 计算点 依据和生成 2n 1 个点 1 1 i kk 1 1kk x 1 1kk p 1 1 i kk 在 UT 变换时 取尺度参数 0 1 2 2in 01 0 0 2 2 计算点 即 i k k 1 1 2 1 0 2 1 1 11 0 0 1 2 2 ii k kkkk n mi k kik k i n T cii k kik kk kk kk kk i in f x p x xQ 3 计算点通过量测方程对的传播 即 1 k k x 1k k p k x 4 计算输出的一步提前预测 即 5 获得新的量测后 进行滤波更新 精品文档 3欢迎下载3欢迎下载3欢迎下载 2 扩展卡尔曼滤波算法分析 对于讨论的非线性系统 由于状态方程为线性的 1 k k kkk kkk xf xw zhxv 定义 k 1 k k k k k x xx fx f x k 1 k k k k k x xx hx h x 由于系统状态方程为线性的 则 kk x fF 而量测方程为非线性的 对其关于求偏导 得到 k x k 1 2222 k 2222 0000 0000 k k kk kkkk k k kk kkkk xy xyxy yx xyxy x xx hx h x EKF 算法步骤如下 k 时刻的一步提前预测 111 1 k kkkk xfx 状态预测误差协方差阵为 11 111 11 T T k kkkkkkkkkkk xx pf pfQF pFQ 卡尔曼滤波增益为 1 1 TT kk kkkk kkk K xxx phh phR 精品文档 4欢迎下载4欢迎下载4欢迎下载 在 k 时刻得到新的量测后 状态滤波的更新公式为 1 1 k kk kkkkk k K xxzhx 状态滤波协方差矩阵为 1 x k kkkk k K pIhp 三 实验仿真与结果分析三 实验仿真与结果分析 假设设系统噪声具有协方差阵 kw 2 2 2 2 100000 010000 000 1000 0000 100 00000 010 000000 01 k Q 具有协方差阵 二者不相关 观测次数 N 50 采样 kv 2 2 01 0 0 05 k R 时间为 t 0 5 初始状态 则生成的运动 0 1000 5000 10 50 2 4 T x 轨迹如图 1 所示 图 1 M 的轨迹图 1 t 0 5 时 UKF 和 EKF 滤波结果比较 我们将 UKF 和 EKF 滤波算法进行比较 如图 2 所示 为了方便对比 我们 将测量值得到的距离和角度换算到笛卡尔坐标系中得到 x y 测量值 直观的可 以看到 UKF 算法滤波结果优于 EKF 算法 精品文档 5欢迎下载5欢迎下载5欢迎下载 图 2 滤波结果对比图 下面定量分析滤波结果 首先计算 UKF 和 EKF 滤波值得到的位置 与该时刻的实际位置的距离 ukfukf yx ekfekf yx yx 对该模型做 50 22 yyxxd ukfukfukf 22 yyxxd ekfekfekf 次蒙特卡洛仿真 得到各个测量点 时刻 的距离均方根误差 如图 3 所示 在各个测量时刻 UKF 滤波结果优于 EKF 图 3 t 0 5 时各个测量点的距离 RMSE 对比图 精品文档 6欢迎下载6欢迎下载6欢迎下载 2 采样间隔 t 对滤波结果的影响 下面讨论不同的采样间隔 t 对滤波结果的影响 分别取 t 0 1 1 0 1 5 得到滤波结果与 RMSE 如下图所示 图 4 采样时间 t 0 1 时结果 图 5 采样时间 t 1 0 时结果 图 6 采样时间 t 1 5 时结果 从上面的 3 张图可以看到 在采样间隔 t 不太大时 0 1 1 0 EKF 和 UKF 精品文档 7欢迎下载7欢迎下载7欢迎下载 算法均能跟踪目标 且 UKF 算法滤波精度优于 EKF 算法 而当 t 1 5 甚至更大 时 EKF 算法滤波不收敛 而 UKF 算法跟踪精度变化不大 对于 EKF 和 UKF 算 法 在不同的 t 时 我们分别取其滤波协方差阵对角线的第二个元素 即 y 方 向位置方差 作出位置方差变化图如下 图 7 不同采样间隔的 y 方向位置滤波方差变化图 出现上述现象的原因为当采样间隔 t 增大时 非线性函数 Taylor 展开式的 高阶项无法忽略 EKF 算法线性化 一阶展开 使得系统产生较大的误差 导 致了滤波的不稳定 由于 UKF 算法可以精确到二阶或者三阶 Taylor 展开项 所 以这种现象不明显 但是当 t 进一步增大 尤其是跟踪目标的状态变化剧烈时 更高阶项误差影响不可忽略 进而 UKF 算法也会发散导致无法跟踪目标 3 测量误差对滤波结果的影响 取采样间隔不变 如 t 0 5s 对于不同的测量误差 分析其对 EKF 和 k R UKF 算法滤波结果的影响 分别取 2 1 001 0 0 010 k R 2 2 001 00 0500 k R 结果如下 精品文档 8欢迎下载8欢迎下载8欢迎下载 图 8 测量误差阵为 R1k 时滤波结果 图 9 测量误差阵为 R2k 时滤波结果 由上面两图对比可知 当测量误差较小时 UKF 滤波精度优于 EKF 当测量 误差较大时 UKF 和 EKF 滤波精度相差不大 综合以上分析可以看到 UKF 算法对于解决非线性模型滤波问题时 相对 于 EKF 算法 它不需要计算雅克比矩阵 具有较好的跟踪精度 而且在非线性 严重或者高阶误差引入时 会推迟或延缓滤波发散 因此在实际中得到了广泛 的应用 精品文档 9欢迎下载9欢迎下载9欢迎下载 附 m 代码 注 注 UTUT 变换及变换及 UKFUKF 函数均来自于函数均来自于 YiYi CaoCao atat CranfieldCranfield University University 04 01 200804 01 2008 functionfunction y Y P Y1 ut f X Wm Wc n R y Y P Y1 ut f X Wm Wc n R Unscented Transformation L size X 2 y zeros n 1 Y zeros n L for k 1 L Y k f X k y y Wm k Y k end Y1 Y y ones 1 L P Y1 diag Wc Y1 R functionfunction X sigmas x P c X sigmas x P c Sigma points around reference point A c chol P Cholesky 分解 Y x ones 1 numel x X x Y A Y A functionfunction x P ukf fstate x P hmeas z Q R x P ukf fstate x P hmeas z Q R UKF Unscented Kalman Filter for nonlinear dynamic systems 精品文档 10欢迎下载10欢迎下载10欢迎下载 L numel x numer of states m numel z numer of measurements alpha 1e 2 default tunable ki 0 default tunable beta 2 default tunable lambda alpha 2 L ki L scaling factor c L lambda scaling factor Wm lambda c 0 5 c zeros 1 2 L weights for means Wc Wm Wc 1 Wc 1 1 alpha 2 beta weights for covariance c sqrt c X sigmas x P c sigma points around x x1 X1 P1 X2 ut fstate X Wm Wc L Q unscented transformation of process z1 Z1 P2 Z2 ut hmeas X1 Wm Wc m R unscented transformation of measurments P12 X2 diag Wc Z2 transformed cross covariance K P12 inv P2 x x1 K z z1 state update P P1 K P12 covariance update functionfunction P k X k ekf f h Q R Z x P P k X k ekf f h Q R Z x P t 1 fx 1 0 t 0 t 2 2 0 0 1 0 t 0 t 2 2 0 0 1 0 t 0 精品文档 11欢迎下载11欢迎下载11欢迎下载 0 0 0 1 0 t 0 0 0 0 1 0 0 0 0 0 0 1 一步提前预测值和预测误差的协方差阵分别是 x 1 f x k 1 时刻对 k 时刻 x 值的预测 P k k 1 fx P fx Q k 1 时刻对 k 时刻 p 值的预测 hx x 1 1 sqrt x 1 1 2 x 1 2 2 x 1 2 sqrt x 1 1 2 x 1 2 2 0 0 0 0 x 1 2 x 1 1 2 x 1 2 2 x 1 1 x 1 1 2 x 1 2 2 0 0 0 0 获取 k 时刻测量值 z 后 滤波更新值和相应的滤波误差的协方差矩阵 K k P k k 1 hx inv hx P k k 1 hx R k 时刻 kalman 滤波增益 X k x 1 K k Z h x 1 P k P k k 1 K k hx P k k 1 ukf test mukf test m clear clc n 6 t 0 5 MC 50 Q 1 0 0 0 0 0 0 1 0 0 0 0 0 0 0 01 0 0 0 0 0 0 0 01 0 0 0 0 0 0 0 0001 0 0 0 0 0 0 0 0001 过程噪声协方差阵 R 100 0 0 0 001 2 量测噪声协方差阵 f x x 1 t x 3 0 5 t 2 x 5 x 2 t x 4 0 5 t 2 x 6 x 3 t x 5 x 4 t x 6 精品文档 12欢迎下载12欢迎下载12欢迎下载 x 5 x 6 x1 为 X 轴位置 x2 为 Y 轴位置 x3 x4 分别 X Y 轴的速度 x5 x6 为 两方向的加速度 h x sqrt x 1 2 x 2 2 atan x 2 x 1 measurement equation s 1000 5000 10 50 2 4 x0 s sqrtm Q randn n 1 initial state with noise P0 100 0 0 0 0 0 0 100 0 0 0 0 0 0 1 0 0 0 0 0 0 1 0 0 0 0 0 0 0 1 0 0 0 0 0 0 0 1 initial state covraiance N 50 total dynamic steps ukV zeros n N ukf estmate ekV zeros n N sV zeros n N actual zV zeros 2 N 测量值 ekx zeros MC N eky zeros MC N eux zeros MC N euy zeros MC N for i 1 N sV i f s s sV i end 精品文档 13欢迎下载13欢迎下载13欢迎下载 plot sV 1 sV 2 k title M 的弹道图 for mc 1 MC uP P0 eP P0 ux x0 ek x x0 for k 1 N z h sV k sqrtm R randn 2 1 测量值 measurments zV k z save measurment ux uP ukf f ux uP h z Q R ukf Pukf k uP 2 2 ukV k ux P k ek x ekf f h Q R z ek x eP ekV k ek x Pekf k P k 2 2 end ekx mc ekV 1 sV 1 eky mc ekV 2 sV 2 eux mc ukV 1 sV 1 euy mc ukV 2 sV 2 end a