UKF滤波算法

《UKF滤波算法》由会员分享，可在线阅读，更多相关《UKF滤波算法（43页珍藏版）》请在金锄头文库上搜索。

1、非线性系统非线性系统 UKFUKF滤波算法滤波算法 主要内容 非线性状态估计原理 MMSE Uscented变换 UT UKF滤波算法 应用比较 非线性状态估计原理非线性状态估计原理非线性状态估计原理非线性状态估计原理 MMSEMMSE 非线性状态估计原理 离散系统 不管条件密度函数的特性如何 最小均方估计就是条件均值 非 线性状态滤波过程的实现包括一步预测一步预测与测量 修正 测量 修正两个阶段 kkk kkkk vxhy wxfx u 1 y x y x p y x y x E 一步预测一步预测 根据所有过去时刻的测量信息对状态作最 小方差估计 状态估计质量的优劣利用一步预测误差 协方差矩

2、阵描述 1 k kk EYxx 1 k T kkkkk EPYxxxx 测量修正测量修正 获得当前时刻的测量信息后 对状态预测 估值进行修正 得到状态的最优估计值 描述最优状态估值质量优劣的误差协方差 阵确定如下 kkkk k kk KEyyxYxx kPkPK yxyk 1 T kykk k T kkkkk KkPKPEP Yxxxx 1 k kk EYyy 1 k T kkkkxy hEkPYxyxx 1 k T kkkky hhEkPYxyxy EKF的不足 必须求非线性函数的Jacobi矩阵 对于模型复 杂的系统 比较复杂且容易出错 引入线性化误差 对非线性强度高的系统 容 易导致滤波

3、效果下降 基于上述原因 为了提高滤波精度和效率 以 满足特殊问题的需要 就必须寻找新的逼近方 法 随机状态变量沿非线性函数的 传播问题是非线性滤波的关 键 注意注意 新思路 近似非线性函数的概率密度分布比 近似非线性函数更容易 近似非线性函数的概率密度分布比 近似非线性函数更容易 因此 使用采样方法近似非线性分布来解决非 线性滤波问题的途径目前得到了人们的广泛关 注 粒子滤波粒子滤波 使用参考分布 随机产生大量粒子 近似 状态的后验概率密度 得到系统的估计 问题 1 计算量甚大 为EKF的若干数量 阶 2 若减少粒子数 估计精度下降 UKF滤波滤波 以UT变换为基础 采用卡尔曼滤波器框 架 采

4、样形式为确定性采样 在减少采样粒子 点数的同时保证逼近精度 UnscentedUnscented变换变换变换变换 Unscented TransformationUnscented Transformation Unscented变换 1 1 构造 构造 构造 构造SigmaSigma点点点点 根据随机向量 x 的统计量和 构造 Sigma点集 为尺度参数 调整它可以提高逼近精度 用这组采样点可以近似表示状态 x 的高斯分 x x P 0 2 1 1 ix nniPnx niPnx i x i x i i 布 2 2 对 对 对 对SigmaSigma点进行非线性变换点进行非线性变换点进行非线

5、性变换点进行非线性变换 对所构造的点集进行非线性变换 得到变换后的Sigma点集 变换后的Sigma点集即可近似地表示 的分布 i f nifY ii 2 1 0 i Y xfy 3 3 计算 计算 计算 计算 y y 的均值和方差的均值和方差的均值和方差的均值和方差 对变换后的Sigma点集进行加权处理 从而得到输出量的均值和方差 和分别为计算的均值和方差所用加权 i Y y n i i m i YWy 2 0 n i T ii c iy yYyYWP 2 0 m i W c i W y nW m 0 2 0 1nW c ninWW c i m i 212 其中 在均值和方差加权中需要确定

6、和共3个参 数 它们的取值范围分别为 确定周围Sigma点的分布 通常设为一个较小 的正数 为第二个尺度参数 通常设置为0或3 n 为状态分布参数 对于高斯分布是最优的 如果状态变量是单变量 则最佳的选择是 适当调节 可以提高估计均值的精度 调节可适当调节 可以提高估计均值的精度 调节可 以提高方差的精度 以提高方差的精度 nn 2 x 4 11 e 2 0 Unscented变换原理图 m i W a f Weighted Sample Meanx c i W y Weighted Sample Covariance Px Py n PP xxi axaxx i Y UT 示意图 UT UT

7、 Uscented变换的特点 1 对非线性函数的概率密度分布进行近 似 而不是对非线性函数进行近似 即使系统 的模型复杂 也不增加算法实现的难度 2 所得到的非线性函数的统计量的准确性 可以达到三阶 泰勒展开 3 不需要计算Jacobi矩阵 可以处理不可导 非线性函数 UKFUKF滤波算法滤波算法滤波算法滤波算法 UKF实现思想 UKF Unscented transform Kalman Filter 即 UKF 可以看作是基于 UT 技术的卡尔曼滤 波器 在卡尔曼滤波算法中 对于一步预测 方程 使用UT变换来处理均值和协方差的非 线性传递 就成为UKF算法 状态的时间更新 选定状态的 2n

8、 1 个 Sigma点 n 为状态维 数 利用 UT 技术计算状态的后验均值和方差 状态的测量更新 利用标准的 Kalman 滤波的测量更新 但使用 的公式有所不同 UT EKF Linearized EKF Unscented Transform ii f APAPxfy x T y Sigma Points UT mean UT covariance transformed sigma points xfAPA x T 两类非线性系统模型 1 加性噪声 加性噪声 2 噪声隐含 噪声隐含 kkk wxfx 1 kkk vxhy 1kkk wxfx kkk vxhy 简化UKF滤波算法 加性噪

9、声 对于非线性系统 假定状态为高斯随机矢量 过程噪声与测量噪 声的统计特性为 kkk wxfx 1 kkk vxhy 0 kk QNw 0 kk RNv 00 xEx 1 1 初始化 初始化 初始化 初始化 T xxxxEP 00000 2 2 状态估计 状态估计 状态估计 状态估计 1 计算计算Sigma点点 nniPnx niPnx x i kk i k i kk i k kk 2 1 1 111 111 1 0 1 2 时间传播方程时间传播方程 i k i kk f 11 n i i kk m ik Wx 2 0 1 kk i kk n i k i kk c ikx QxxWP T 1

10、2 0 1 i kk n i m ik Wy 1 2 0 i kk i kk h 1 1 3 测量更新方程测量更新方程 k T k i kkk n i i kk c iky RyyWP 1 2 0 1 T k i kkk i kk n i c ikxy yxWP 1 1 2 0 1 kykxy PPK kkkk yyKxx T kykxkx KKPPP 计算量 与 EKF 的计算量在同一个数量阶 对于 n 维 系统 为 O n3 UKF 和 EKF 的计算量之比大致为 UKF EKF 3 1 UKF 的主要计算量在于选取 Sigma 点时的方 根分解运算 所以优化计算可以从分解方 式入手 好的

11、分解方式可以减小计算量 1 k P UKF的优点 不必计算 Jacobi 矩阵 不必对非线性系统函 数 f x 进行任何形式的逼近 预测阶段只是标准的线性代数运算 矩阵方根 分解 外积 矩阵和向量求和 系统函数可以不连续 随机状态可以不是高斯的 计算量和 EKF 同阶 扩维UKF滤波算法 噪声隐含 若过程噪声与测量噪声是隐含在系统中的 即 系统方程为 这时需要对状态变量进行扩展 得增广状态 1kkk wxfx kkk vxhy T T k T k T kka vwxx 增广状态的均值为 T T l T m T kka xx 11 00 其中 和分别为过程噪声和观测噪声的维数 ml 增广状态的方

12、差为 R Q P P kx ka 00 00 00 1 1 初始化 初始化 初始化 初始化 00 xEx T x xxxxEP 00000 T T l T m T a xEx 1100 00 R Q P P x a 00 00 00 0 0 2 2 状态估计 状态估计 状态估计 状态估计 1 计算计算Sigma点点 根据和 构造增广Sigma点 1 ka x 1 ka P NNiPNx NiPNx x i kaka i ka i kaka i ka kaka 2 1 1 1 1 1 1 1 1 1 0 1 这里 为增广状态的维数 且lmnN T T v T w T xa 2 时间传播方程时间传

13、播方程 i kw i kx i kkx f 1 1 1 i kkx N i m ik Wx 1 2 0 N i T k i kkxk i kkx c ikx xxWP 2 0 1 1 i kv i kkx i kk h 1 1 1 i kk N i m ik Wy 1 2 0 3 测量更新方程测量更新方程 N i T k i kkk i kk c iky yyWP 2 0 1 1 N i T k i kkk i kkx c ikxy yxWP 2 0 1 1 1 kykxy PPK kkkk yyKxx T kykxkx KKPPP 两类UKF算法的比较 处理加性噪声的简化UKF的Sigma点

14、较处理隐 含噪声的扩维UKF要少许多 简化UKF的Sigma点数 2n 1 扩维UKF的Sigma点数 2N 1 2 n m l 1 由此 简化UKF的计算量较之扩维UKF大大降 低 应用实例 模型 卫星姿态确定系统 其中 2 1 1 ttqttqf 0 1 3 34 2 1 tdJ Itf ttqf tGdtxftx 2 1 kv rkqA rkqA kvkxhky 1 2 tJttNJtf 偏航角估计误差 051015202530 1 0 5 0 0 5 1 1 5 x 10 3 偏航角误差 deg EKF UKF 时间 s 滚动角估计误差 051015202530 14 12 10 8

15、6 4 2 0 2 x 10 4 EKF UKF 时间 s 滚动角误差 deg 俯仰角估计误差 051015202530 1 5 1 0 5 0 0 5 1 x 10 3 时间 s EKF UKF 俯仰角误差 deg 主要参考文献 S Julier and J K Uhlmann A General Method for Approximating Nonlinear Transformations of Probability Distributions Technical Report Robotics Research Group Department of Engineering Sc

16、ience University of Oxford 1994 S Julier and J K Uhlmann A New Approach for Filtering Nonlinear Systems Proc of the 1995 American Control Conference Seattle Washington pp 1628 1632 S Julier and J K Uhlmann The Scaled Unscented Transformation Proc of the American Control Conference Anchorage AK May 8 10 2002 S Julier J K Uhlmann etc A New Method for the Nonlinear Transformation of Means and Covariances in Filters and Estimators IEEE Trans A C 2000 45 3 477 482 S Julier J K Uhlmann Reduced Sigma P