扩展Kalman滤波EKF和无迹卡尔曼滤波ukf课堂.ppt

EKF 与 UKF 1 一、背景 普通卡尔曼滤波是在线性高斯情况下利用最小均方误差准则获得 目标的动态估计，适应于 过程和测量都属于线性系统 ， 且 误差符 合高斯分布 的系统。 但是实际上很多系统都存在一定的非线性， 表现在 过程方程 （状态方程）是非线性 的，或者 观测与状态之间 的关系（测量方程）是非线性的 。这种情况下就不能使用一般的卡 尔曼滤波了。解决的方法是将非线性关系进行 线性 近似，将其转化 成线性问题。 对于非线性问题线性化常用的两大途径： （ 1 ） 将非线性环节线性化，对高阶项采用忽略或逼近措施；（ EKF ） （ 2 ）用采样方法近似非线性分布 . ( UKF ) 2 2020/4/26 二、扩展 Kalman 滤波 (EKF) 算法 ? EKF 算法是一种近似方法，它将非线性模型在状态 估计值附近作泰勒级数展开，并在一阶截断，用得 到的一阶近似项作为原状态方程和测量方程近似表 达形式，从而实现线性化同时假定线性化后的状态 依然服从高斯分布，然后对线性化后的系统采用标 准卡尔曼滤波获得状态估计。采用局部线性化技术， 能得到问题局部最优解，但它能否收敛于全局最优 解，取决于函数的非线性强度以及展开点的选择。 3 2020/4/26 二、扩展 Kalman 滤波 (EKF) 算法 ? 假定定位跟踪问题的非线性状态方程和测量方程如 下 : ? 在最近一次状态估计的时刻，对以上两式进 行线性化处理，首先构造如下 2 个矩阵 : ) 2 ( .......... .......... ) ( ) 1 .....( .......... ) ( 1 k k k k k k V X h Y W X f X ? ? ? ? ? ) 4 .( .......... ) ( ) ( ) 3 .......( ) ( ) 1 ( ) 1 ( ) ( ? ? ? ? ? ? ? ? ? ? k k X X k k k X X k X X h K H X X f k k F 4 2020/4/26 二、扩展 Kalman 滤波 (EKF) 算法 ? 将线性化后的状态转移矩阵和观测矩阵代入到标 准卡尔曼滤波框架中，即得到扩展卡尔曼滤波。 ? 因为 EKF 忽略了非线性函数泰勒展开的高阶项， 仅仅用了一阶项，是非线性函数在局部线性化的结果， 这就给估计带来了很大误差，所以只有当系统的状态 方程和观测方程都接近线性且连续时， EKF 的滤波结 果才有可能接近真实值。 EKF 滤波结果的好坏还与状 态噪声和观测噪声的统计特性有关，在 EKF 的递推滤 波过程中，状态噪声和观测噪声的协方差矩阵保持不 变，如果这两个噪声协方差矩阵估计的不够准确，那 就容易产生误差累计，导致滤波器发散。 EKF 的另外 一个缺点是初始状态不太好确定，如果假设的初始状 态和初始协方差误差较大，也容易导致滤波器发散。 5 2020/4/26 二、扩展 Kalman 滤波 (EKF) 算法 ? Matlab 程序： ? function test_ekf ? 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]; % 状态模拟的初值 ? for k=2:len ? x = 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; 6 2020/4/26 二、扩展 Kalman 滤波 (EKF) 算法 ? vy = vy + (ky*vy^2-g+day*randn(1))*Ts; ? X(k,:) = [x, vx, y, vy]; ? end ? figure(1), hold off, plot(X(:,1),X(:,3),-b), grid on ? % figure(2), plot(X(:,2:2:4)) ? % 构造量测量 ? mrad = 0.001; ? dr = 10; dafa = 10*mrad; % 量测噪声 ? for k=1:len ? r = sqrt(X(k,1)^2+X(k,3)^2) + dr*randn(1,1); ? a = atan(X(k,1)/X(k,3)) +