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.
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
