扩展Kalman滤波(EKF)和无迹卡尔曼滤波(ukf).
合集下载
相关主题
- 1、下载文档前请自行甄别文档内容的完整性,平台不提供额外的编辑、内容补充、找答案等附加服务。
- 2、"仅部分预览"的文档,不可在线预览部分如存在完整性等问题,可反馈申请退款(可完整预览的文档不适用该条件!)。
- 3、如文档侵犯您的权益,请联系客服反馈,我们会尽快为您处理(人工客服工作时间:9:00-18:30)。
h( X k ) H (K ) X
X X ( k k 1)
.......... .(4)
二、扩展Kalman滤波(EKF)算法
将线性化后的状态转移矩阵和观测矩阵代入到标 准卡尔曼滤波框架中,即得到扩展卡尔曼滤波。 因为EKF忽略了非线性函数泰勒展开的高阶项, 仅仅用了一阶项,是非线性函数在局部线性化的结果, 这就给估计带来了很大误差,所以只有当系统的状态 方程和观测方程都接近线性且连续时,EKF的滤波结 果才有可能接近真实值。EKF滤波结果的好坏还与状 态噪声和观测噪声的统计特性有关,在EKF的递推滤 波过程中,状态噪声和观测噪声的协方差矩阵保持不 变,如果这两个噪声协方差矩阵估计的不够准确,那 就容易产生误差累计,导致滤波器发散。EKF的另外 一个缺点是初始状态不太好确定,如果假设的初始状 态和初始协方差误差较大,也容易导致滤波器发散。
EKF与UKF
一、背景
普通卡尔曼滤波是在线性高斯情况下利用最小均方误差准则获得
目标的动态估计,适应于过程和测量都属于线性系统, 且误差符
合高斯分布的系统。 但是实际上很多系统都存在一定的非线性, 表现在过程方程 (状态方程)是非线性的,或者观测与状态之间 的关系(测量方程)是非线性的。这种情况下就不能使用一般的卡 尔曼滤波了。解决的方法是将非线性关系进行线性近似,将其转化
二、扩展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)) + dafa*randn(1,1); Z(k,:) = [r, a]; end
二、扩展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;
二、扩展Kalman滤波(EKF)算法
figure(1), hold on, 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 = zeros(4,1); Pk = 100*eye(4); X_est = X; for k=1:len Ft = 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
成线性问题。 对于非线性问题线性化常用的两大途径:
(1) 将非线性环节线性化,对高阶项采用忽略或逼近措施;(EKF)
(2)用采样方法近似非线性分布. ( UKF)
二、扩展Kalman滤波(EKF)算法
EKF算法是一种近似方法,它将非线性模型在状态 估计值附近作泰勒级数展开,并在一阶截断,用得 到的一阶近似项作为原状态方程和测量方程近似表 达形式,从而实现线性化同时假定线性化后的状态 依然服从高斯分布,然后对线性化后的系统采用标 准卡尔曼滤波获得状态估计。采用局部线性化技术, 能得到问题局部最优解,但它能否收敛于全局最优 解,取决于函数的非线性强度以及展开点的选择。
二、扩展Kalman滤波(EKF)算法
figure(1), plot(X_est(:,1),X_est(:,3), '+r') xlabel('X'); ylabel('Y'); title('ekf simulation'); legend('real', 'measurement', 'ekf estimated'); %%%%%%%%%%%%%%%%%%%%子程序%%%%%%%%%%%%%%%%%%% function F = 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)算法
假定定位跟踪问题的非线性状态方程和测量方程如 下: X k 1 f ( X k ) Wk .......... .....( 1)
Yk h( X k ) Vk
.......... .......... (2)
在最近一次状态估计的时刻,对以上两式进 行线性化处理,首先构造如下2个矩阵: f ( X k ) F (k 1 k ) .......( 3) X X (k k ) X