Kalman Filter’s Daddy!

Aaaaah! Research! Oh how much I love thee, and how much I HATE thee!

Khair… I have been trying to understand how actually Kalman filters work for a while now and Believe me when I tell you they aint easy! I went through hell to get the knots in my brain straightened out! I read a book called Introduction to Random Signals and Applied Kalman Filtering in about two days, at least the first 6 chapters! I reference a gazillion papers and other material online, talked to a mathematician, run simulations and what not… the list goes on. Heck I was so frustrated and unsure of myself that I had to relearn my 4th semester signals and systems course (the state spaces part).

Who knew in the end good old fashioned tinkering would make it work. So! I have decided that I would nt let the other people looking for a quick KF fix roam around aimlessly like particles in Brownian motion in the dark…

Ok first thing I learnt was that you need the theory, I read this fairly complicated and annoying book mentioned above… After reading the chapter like 3 times, I was able to grasp (somewhat) what it was trying to say. But the prolem is (I think that the equations dont directly apply for some reason when implementing the damn thing…. Plus most of the time you would be wondering how to calculate and use most of the variables. So yes in short its not the way to go if you are thinking of applying it in the near future.

But the reading part is very necessary, cuz you definitely need to know the why  part… for the how, go to this fairly good article on embedded.com. Ok this article also get confusing as it does nt tells you a LOT of things… like how will you be calculating the noise covariances and what is the difference between measurement noise and process noise.

To answer these very relevant questions I m writing this article.

First of I m going to show a very simple example…. nothing complicated… What I m going to do is collect data from a single accelerometer and filter its output rather than the position data calculated from it. This reduces most of our state space matrices to 1. The commonly used system eqs are

x_hat_dot     =     A_mat * x    +    B_mat * u     +     w

y              =               C_mat * x        +        z

Ok…. to explain what these are you ll need to read some basic text on state spaces, I would recommend Signals, Systems, and Transforms by C.L.Phillips, J.M.Parr and E.A.Riskin. I am not going to go through that over here… NO CHANCE IN HELL!

So moving on to Kalman filters and our simple filtering app…. What I did was simply record a lot of data from the accelerometer and calculate the mean and variance/standard deviation parameters for the data. There is a small problem where that needs to be address. most sensors give the zero force point by Vcc/2, and since the filter parameters require the mean to be near zero and the SD to be around it, a transformation is in order.The easiest say is to subtract the average and divide by the gravity value. So thats that…. Now we have our sensor data and its parameters. the measurement and process noise are supposed to be different for the system. But while playing around with the values I found that if you Q as SD ^ 2 and R as SD you get an awesomely filtered result. Now dont ask me why the works. But theoretically R should be the SD and Q should be calculated from the A matrix and some other info about the process.

So the simple program that I ran in MATLAB is:

 

phi_k = 1;
H_k = 1;
R_k = (7.29738/(3113.08-2131.33));
P_min_k (1) = 0;
Q_k = R_k ^ 2;
P_k(1) = 1;
x_hat (1) = 0;
n = size1/NumberOfColumns;
for i = 1:n -1
%     K_k(i) = P_min_k (i) * H_k * inv(H_k * P_min_k (i) * H_k + R_k);
%
%     x_hat(i) = x_hat_min + K_k(i) * (da1(i,1) - H_k*x_hat_min);
%     x_hat(i+1) = phi_k * x_hat(i);
%
%     P_k (i) = (1 - K_k(i) * H_k) * P_min_k(i);
%     P_min_k(i+1) = phi_k * P_k(i) * phi_k + Q_k;
K_k(i) = phi_k * P_k (i) * H_k * inv(H_k * P_k (i) * H_k + R_k);
x_hat(i+1) = phi_k * x_hat(i) + K_k(i) *(da1(i,1) - H_k*x_hat(i));
P_k(i+1) = phi_k * P_k(i) * phi_k + Q_k - phi_k * P_k(i) * H_k * inv(R_k + P_k(i)) * H_k * P_k(i) * phi_k;
end
figure;
hold on;
plot(K_k, '-r');
plot(P_k, '-b');
plot(P_min_k, '-g');
xlabel('Time/sample');
ylabel('Bits');
title(strFileName);
figure;
hold on;
plot(da1(:,1), '-g');
% plot(x_hat_min, '-r');
plot(x_hat, '-b');
xlabel('Time/sample');
ylabel('Bits');
title(strFileName);

 

phi_k = 1;H_k = 1;R_k = (7.29738/(3113.08-2131.33));P_min_k (1) = 0;Q_k = R_k ^ 2;P_k(1) = 1;

x_hat (1) = 0;n = size1/NumberOfColumns;
for i = 1:n -1%     K_k(i) = P_min_k (i) * H_k * inv(H_k * P_min_k (i) * H_k + R_k);%     %     x_hat(i) = x_hat_min + K_k(i) * (da1(i,1) - H_k*x_hat_min);%     x_hat(i+1) = phi_k * x_hat(i);%     %     P_k (i) = (1 - K_k(i) * H_k) * P_min_k(i);%     P_min_k(i+1) = phi_k * P_k(i) * phi_k + Q_k;    K_k(i) = phi_k * P_k (i) * H_k * inv(H_k * P_k (i) * H_k + R_k);        x_hat(i+1) = phi_k * x_hat(i) + K_k(i) *(da1(i,1) - H_k*x_hat(i));        P_k(i+1) = phi_k * P_k(i) * phi_k + Q_k - phi_k * P_k(i) * H_k * inv(R_k + P_k(i)) * H_k * P_k(i) * phi_k;
end
figure;hold on;
plot(K_k, '-r');plot(P_k, '-b');plot(P_min_k, '-g');xlabel('Time/sample');ylabel('Bits');title(strFileName);
figure;hold on;
plot(da1(:,1), '-g');% plot(x_hat_min, '-r');plot(x_hat, '-b');xlabel('Time/sample');ylabel('Bits');title(strFileName);

 

 

Keep in mind that the Data was already loaded into the MATLAB workspace. The figure of interest is

kalmanWorks like a charm.

 

Dont ask me how or why though… I m just happy it does!

Advertisements

23 Responses to Kalman Filter’s Daddy!

  1. Ok I m in no mood to correct the typo’s and the mistakes…. I m going to sleep now! Good luck trying to understand the mumbo jumbo!

  2. anas imtiaz says:

    And Im in no mood of reading this

  3. H says:

    I skimmed through it to get a jist of this. But I’m going to be a typical engineering and ask straight off before thoroughly reading jargon. Practical applications?

    • Yeah…. it sounds confusing but this filter is probably the best filter of all… it reduced the men square error for the system. Most filters limit the noise to a specific band, reducing it only in he stop band.

  4. says:

    All I understood was that one little for statement.
    😀

  5. Sumera says:

    That gave me a headache

  6. Safiullah says:

    @stb: NERD… Took me a while to learn the abc’s, then the spelling and the pronunciation, and then getting around to sayit…. I learned it all for you! Happy?

    @Sumera: Believe me, If you did the real thing, your brain would shrink to the size of a peanut!Naturally your head would implode due to the vacuum inside.

  7. PD says:

    That just went… over my head. 😛

  8. Huda says:

    Safi, I didn’t understand a single thing. Oh man, I feel so dumb 😐

  9. huda why do you feel dumb?

    safi’s like one of those nerds they’re gonna fosilise and put in museums

    who cares if you can’t understand his “research”??

  10. Leena S. says:

    i refuse to read this all abhi! will come back later to read it

  11. AD says:

    for the first time in so many days… you’ve driven me crazy

  12. pinkkay says:

    tumhari side pe update ka rawwaj hai k nahin? 😛

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

%d bloggers like this: