Ref. Open: EKF

2014년 9월 24일 수요일

EKF vs. UKF 비선형 상태 변환 성능 비교

필터에서 상태 천이 함수가 선형인 경우엔 Kalman filter를 사용합니다. 상태 천이 함수가 비선형인 경우에는 EKF, UKF, PF를 사용하게 되는데, 이 때 간단한 비선형 천이 함수를 이용하여 EKF와 UKF의 성능이 어떤 차이가 발생하는지를 확인해보고자 합니다.

다음에 제시한 예제는 $x, y$ domain에서 $ \rho, \theta $로 변환하는 기본적인 비선형 천이 함수를 이용해 EKF와 UKF가 예측한 $2\sigma$를 표현한 타원을 보면 직관적으로 그 차이를 이해할 수 있습니다.

http://refopen.blogspot.com/2014/09/ekf-vs-ukf.html

Magnus Nørgaard는 derivative-free filter인 UKF가 taylor series의 2차까지 근사화하므로 비선형 함수에서 더욱 정확한 성능을 보장한다고 주장했습니다.

http://www.sciencedirect.com/science/article/pii/S0005109800000893

EKF vs. UKF 비선형 상태 변환 성능 비교 예제

filterComparison

% EKF vs. UKF
% 2014. 9. 24
% refopen.blogspot.com
% This example intuitively shows that the difference between EKF and UKF
% when state transition is non-linear function.

function filterComparison()

Create real data

Create some real data points(1000points)
$\sigma_x = 5, \sigma_y = 3$

sigmax = 5;
sigmay = 3;
rx = randn(1000, 1)*sigmax;
ry = randn(1000, 1)*sigmay;

% real data points and MLE parameter estimation
figure(001);
plot(rx, ry, 'r.');
xlabel('x');
ylabel('y');
axis([-15 15 -15 15]);
[x, P] = MLEnPlot(rx, ry);

$r = \sqrt{x^2+y^2}$
$\theta = tan^{-1}\frac{y}{x}$

figure(002);
rho = sqrt(rx.^2 + ry.^2);
theta = atan2(ry, rx);
plot(rho, theta, 'r.');
xlabel('\rho');
ylabel('\theta');
MLEnPlot(rho, theta);
fprintf('press any key to continue...\n');
pause;

press any key to continue...

Extended Kalman Filter

Find out jacobian matrix when it transformed by non-linear state transition matrix.
$J = \left[\begin{array}{cc} \frac{\partial r}{\partial x} & \frac{\partial r} {\partial y} \\ \frac{\partial \theta}{\partial x} & \frac{\partial \theta}{\partial y} \end{array}\right]$
Now, prediction of covariance matrix at the pridiction step $P_{r\theta}=JP_{xy}J^T$

q = x(1).^2 + x(2).^2;
J = [x(1)/sqrt(q) x(2)/sqrt(q); -x(2)/q x(1)/q];

xekf = [sqrt(x(1).^2 + x(2).^2) atan2(x(2), x(1))];
Pekf = J*P*J';

% draw ellipse
e = make_covariance_ellipses(xekf, Pekf);
hold on; plot(e(1, :), e(2, :), 'g-' , 'LineWidth', 3); hold off;
fprintf('press any key to continue...\n');
pause;

press any key to continue...

Unscented Kalman Filter

Set up some values

D = length(x);  % dimension
N = D*2 + 1;

samples are should be generated.

scale = 3;
kappa = scale-D;

% Create unscented sample points
Ps = chol(P)' * sqrt(scale);
ss = [x', repvec(x',D)+Ps, repvec(x',D)-Ps];

$(x, y) \rightarrow (\rho, \theta)$

rho = sqrt(ss(1, :).^2 + ss(2, :).^2);
theta = atan2(ss(2, :), ss(1, :));
x = [rho; theta];

idx = 2:N;
xukf = (2*kappa*x(:,1) + sum(x(:,idx),2)) / (2*scale);
diff = x - repvec(xukf,N);  % weighted sample cov.
Pukf  = (2*kappa*diff(:,1)*diff(:,1)' + diff(:,idx)*diff(:,idx)') / (2*scale);

% draw ellipse
e = make_covariance_ellipses(xukf, Pukf);
hold on; plot(e(1, :), e(2, :), 'k-' , 'LineWidth', 3); hold off;

end

function [x, P] = MLEnPlot(x, y)
phatx = mle(x);
phaty = mle(y);
x = [phatx(1) phaty(1)];
P = [phatx(2) 0; 0 phaty(2)];
e = make_covariance_ellipses(x, P);
hold on; plot(e(1, :), e(2, :), 'b-' , 'LineWidth', 3); hold off;
end

from timbailey's matlab utility

function p= make_covariance_ellipses(x,P)
% compute ellipses for plotting state covariances
N= 60;
inc= 2*pi/N;
phi= 0:inc:2*pi;

lenx= length(x);
lenf= (lenx-3)/2;
p= zeros (2,(lenf+1)*(N+2));

ii=1:N+2;
p(:,ii)= make_ellipse(x(1:2), P(1:2,1:2), 2, phi);

ctr= N+3;
for i=1:lenf
    ii= ctr:(ctr+N+1);
    jj= 2+2*i; jj= jj:jj+1;

    p(:,ii)= make_ellipse(x(jj), P(jj,jj), 2, phi);
    ctr= ctr+N+2;
end
end
%
%

function p= make_ellipse(x,P,s, phi)
% make a single 2-D ellipse of s-sigmas over phi angle intervals
r= sqrtm(P);
a= s*r*[cos(phi); sin(phi)];
p(2,:)= [a(2,:)+x(2) NaN];
p(1,:)= [a(1,:)+x(1) NaN];
end

function x = repvec(x,N)
x = x(:, ones(1,N));
end