Karman Filter Calculation

Hello..

after review the rest of my blog, I just realized that so many post that explaining about IT/UNIX hehehe…

one thing that you should know, I’m electro engineer..wkwwkwkwk

this time, I’ll post about Karman Filter, ok..lets start

Whats The Karman Filter?

first of all, you have to know about LDS( Linear Dynamical System) a partially observed stochastic process with linear dynamics and linear observations, both subject to Gaussian noise. It can be defined as follows, where X(t) is the hidden state at time t, and Y(t) is the observation.

here's the equation :
x(t+1) = F*x(t) + w(t),  w ~ N(0, Q),  x(0) ~ N(X(0), V(0))
y(t)   = H*x(t) + v(t),  v ~ N(0, R)

The Kalman filter is an algorithm for performing filtering on this model, i.e., computing P(X(t) | Y(1), …, Y(t)).
The Rauch-Tung-Striebel (RTS) algorithm performs fixed-interval offline smoothing, i.e., computing P(X(t) | Y(1), …, Y(T)), for t <= T.

The Example of Karman Filter :
Here is a simple example. Consider a particle moving in the plane at constant velocity subject to random perturbations in its trajectory. The new position (x1, x2) is the old position plus the velocity (dx1, dx2) plus noise w.

[ x1(t)  ] =  [1 0 1 0] [ x1(t-1)  ] + [ wx1  ]
[ x2(t)  ]    [0 1 0 1] [ x2(t-1)  ]   [ wx2  ]
[ dx1(t) ]    [0 0 1 0] [ dx1(t-1) ]   [ wdx1 ]
[ dx2(t) ]    [0 0 0 1] [ dx2(t-1) ]   [ wdx2 ]

We assume we only observe the position of the particle.

[ y1(t) ] =  [1 0 0 0] [ x1(t)  ] + [ vx1 ]
[ y2(t) ]    [0 1 0 0] [ x2(t)  ]   [ vx2 ]
                       [ dx1(t) ] 
                       [ dx2(t) ]

Suppose we start out at position (10,10) moving to the right with velocity (1,0). We sampled a random trajectory of length 15. Below we show the filtered and smoothed trajectories.

aima_filtered Smoothed Trajector

The mean squared error of the filtered estimate is 4.9; for the smoothed estimate it is 3.2. Not only is the smoothed estimate better, but we know that it is better, as illustrated by the smaller uncertainty ellipses; this can help in e.g., data association problems. Note how the smoothed ellipses are larger at the ends, because these points have seen less data. Also, note how rapidly the filtered ellipses reach their steady-state (Ricatti) values.

here’s the code to generate the Graphs :

|——————————————————————

% X(t+1) = F X(t) + noise(Q)
% Y(t) = H X(t) + noise(R)

ss = 4; % state size
os = 2; % observation size
F = [1 0 1 0; 0 1 0 1; 0 0 1 0; 0 0 0 1];
H = [1 0 0 0; 0 1 0 0];
Q = 0.1*eye(ss);
R = 1*eye(os);
initx = [10 10 1 0]’;
initV = 10*eye(ss);

seed = 9;
rand(‘state’, seed);
randn(‘state’, seed);
T = 15;
[x,y] = sample_lds(F, H, Q, R, initx, T);

[xfilt, Vfilt, VVfilt, loglik] = kalman_filter(y, F, H, Q, R, initx, initV);
[xsmooth, Vsmooth] = kalman_smoother(y, F, H, Q, R, initx, initV);

dfilt = x([1 2],:) – xfilt([1 2],:);
mse_filt = sqrt(sum(sum(dfilt.^2)))

dsmooth = x([1 2],:) – xsmooth([1 2],:);
mse_smooth = sqrt(sum(sum(dsmooth.^2)))

subplot(2,1,1)
hold on
plot(x(1,:), x(2,:), ‘ks-‘);
plot(y(1,:), y(2,:), ‘g*’);
plot(xfilt(1,:), xfilt(2,:), ‘rx:’);
for t=1:T, plotgauss2d(xfilt(1:2,t), Vfilt(1:2, 1:2, t), 1); end
hold off
legend(‘true’, ‘observed’, ‘filtered’, 0)
xlabel(‘X1’)
ylabel(‘X2’)

subplot(2,1,2)
hold on
plot(x(1,:), x(2,:), ‘ks-‘);
plot(y(1,:), y(2,:), ‘g*’);
plot(xsmooth(1,:), xsmooth(2,:), ‘rx:’);
for t=1:T, plotgauss2d(xsmooth(1:2,t), Vsmooth(1:2, 1:2, t), 1); end
hold off
legend(‘true’, ‘observed’, ‘smoothed’, 0)
xlabel(‘X1’)
ylabel(‘X2’)

|——————————————————————

on the next post, I’ll show you how to implement this Algorithm into Simple Manipulator Robot.

Thank’s for watching..eh, thank you for reading maksudnya. wkwkwk
see you, keep on rock
 

Advertisements
Karman Filter Calculation

Leave a Reply

Fill in your details below or click an icon to log in:

WordPress.com Logo

You are commenting using your WordPress.com account. Log Out / Change )

Twitter picture

You are commenting using your Twitter account. Log Out / Change )

Facebook photo

You are commenting using your Facebook account. Log Out / Change )

Google+ photo

You are commenting using your Google+ account. Log Out / Change )

Connecting to %s