Kalman Filtering

[sdk32285]

Here is a quick intro/quasi-tutorial on kalman filtering (Most people (me included) don't actually understand how it works, they just learn how to implement one)

Kalman filtering can give you the optimal estimate for a linear system. There are many "flavors" of the filter. The two most common types are the generic Kalman Filter which is for linear systems and the Extended Kalman Filter which is for non linear systems.

A quick note before continuing is that state refers to the current position/velocities/values, and action refers to the commanded change in value/torques/etc...

There are two types of equations for the kalman. The first are the time update equations, These equations predict what the current state is based from the last state and the commanded action. The second set of equations known as the measurement update equations looks at your input sensors, how much you trust each sensor, and how much you trust your overall state estimate. I am going to present the equations and than explain the different variables.

TIME UPDATE EQUATIONS
X=Ax + Bu
p=APA' + Q

MEASUREMENT UPDATE QUATIONS
K=pH' (HpH'+R)^-1
x"=X + K(z-HX)
P=(I-KH)p

where
A,B= the process model (A and B are different matrices)
x=current state (or what was the state estimate at the last time step)
x"= current state estimate  (this becomes x for the next iteration)
X= estimate without using the sensors to correct the filters "guess"
u=action
P,p= Covariance, estimate of the error in the current state estimate
Q= Covariance, estimate of the process noise - this is the error from what you command the motors to what actually happens
K=kalman gains - tells the filter how to adjust the state estimate for your system
H=sensor model
R=Covariance, estimate of noise in your sensors/system
z=measurements from the sensors
I=identity matrix
and ' means take the transpose of the matrix

to be continued...

[JesseWelling]

These equations predict what the current state is based from the last state and the commanded action.

To validate my own scattered knowledge of this subject I need to ask for a clarification. I'm not trying to be a nitpicking prick, so please don't take it that way. I'm just trying to get it down in my own mind. Is this a more accurate statement? "These equations predict what the current state is based from the last state and the last commanded action."

That being said, maybe I'm jumping the gun with this question, but what if you have a PID (or PI or PD or P or whatever) control of say a velocity and you update at 50 hz, and you are going to run your Kalman Filter at 10 hz. I'm guessing you include the error of the PID into Q, and  then buffer the last command into the PID u action along with steering commands if you are talking Ackermann model?

And if you are going to get to this in due time, tell me to settle down. Good work by the way, This is interesting! 

[sdk32285]

Here are some matrix notations that I will be using.
[a,b,c,d] = row vector
[a;b;c;d] = column vector
[a,b,c,d] ' = [a;b;c;d]  the  ' means take the transpose
[a,b;c,d]^-1  this means to take the inverse of this n x n matrix
Also something that is useful for debugging is that 2 matrices need to have the same dimensions in order to add them, and to multiply matrices 3 x 4 and 4 x 1 the inner numbers must be the same so 4=4 and the dimensions of the multiplications is 3 x 1

So now that we have the basic equations and what the variables are we can begin trying to figure out what each variable actually is.

So here we go:
A,B = these two variables are probably the two most important variables to get correct. As such I am going to save them for last so I can go into the most detail for them.
x,x",X = These variables represent your state (ie. the things you care about and/or trying to filter) so for example if you are controlling a robotic arm with three joints your state can be [position1;position2;position3;velocity1;velocity2;velocity3] , this should be initialized at what ever you want to start at. if you are treating your start location as the origin that s=[0;0;0;0;0;0]
u = what ever the action is, so for the three joint robotic arm the actions can be [torque1;torque2;torque3]. These values also need to be initialized, I just initialized them as small values so when I use them in my time update equation I get real values and I let my controller (I am using an LQR but you can also use a PID) come up with what the values should be once I iterate a few times.
P,p = these numbers represent how confident the filter is with the solution. The best way to do this is to initialize it as a diagonal matrix and then as the filter runs it will become populated. after running the filter you can look at how the values converge and use them to initialize the filter the next time. So the reason I say that it should be initialized as a diagonal is that each entry directly corresponds to the variance of the state in that row so for the above robotic arm with six states the covariance matrix can be initialized as:
[variance,    0,          0,       0,        0,        0;
    0,       variance,    0,       0,        0,        0;
    0,          0,      variance,  0,        0,        0;
    0,          0,          0,    variance,  0,        0;
    0,          0,          0,       0,     variance,  0;
    0,          0,          0,       0,        0,     variance]

where variance is the square of the std deviation of the error

Q= is the covariance of the process (ie action) noise it is formed similar to the above except that it is a matrix that you determine and does not get updated by the filter. This matrix tells the kalman filter how much error is in each action from the time you issue the commanded torque until it actually happens.
K= this matrix gets updates as part o the measurement update stage
H= is a model of the sensors, this is also pretty hard to determine. I just initialized it as a diagonal identity matrix and tweaked it to improve the final filter results.
R = similar to Q except this defines the confidence in each of the sensors. This is the key matrix for doing sensor fusion
z= this is the measurements that are returned from the sensors at each location. The kalman filter is usually used to clean the noise from these signals or to estimate these parameters when there is no sensor.
I= an Identity matrix (also diagonal)

to be continued...

[sdk32285]

To validate my own scattered knowledge of this subject I need to ask for a clarification. I'm not trying to be a nitpicking prick, so please don't take it that way. I'm just trying to get it down in my own mind. Is this a more accurate statement? "These equations predict what the current state is based from the last state and the last commanded action."

So there are different point of views for explaining the kalman filter, so naturally every one uses a different approach. Simple answer is yes you are correct. The two main approaches are to say that I am looking at my current state and trying to figure out whats next, or you can say that I am at my current state and I am trying to figure out what it is based from my previous state.

what if you have a PID (or PI or PD or P or whatever) control of say a velocity and you update at 50 hz, and you are going to run your Kalman Filter at 10 hz. I'm guessing you include the error of the PID into Q, and  then buffer the last command into the PID u action along with steering commands if you are talking Ackermann model?

I have not really thought of that since I am running my filter at the same rate as my controller. I would imagine that you would take the sum of all your KP's to somehow update Q (maybe scaled by some other matrix), and sum all the steering commands to form the u matrix.

When I finish writing this kalman filter intro I might write one on an LQR controller (linear quadratic controller) that can be used instead of a PID. An LQR allows for greater control of which sensors you trust and you can assign "costs" to each action so that you can make certain movements more favorable to the system. While an LQR kind of works like magic you need to compute the system response matrices (A and B) which can be difficult, however once you compute them for the Kalman filter you can reuse them for the LQR with out much difficulty.

[JesseWelling]

You by chance wouldn't be able to recommend a good book that covers Kalman Filters, and other control theory kind of stuff. I just had my yearly review and they said they would kick in for some high quality books, so I'm just kinda hunting around at the moment.

[Tsukubadaisei]

2 things:
first:
Quote
x,x",X = These variables represent your state (ie. the things you care about and/or trying to filter) so for example if you are controlling a robotic arm with three joints your state can be [position1;position2;position3;velocity1;velocity2;velocity3] , this should be initialized at what ever you want to start at. if you are treating your start location as the origin that s=[0;0;0;0;0;0]
The robotic arm doesnt need to have 3 joints in order to be represented by [position1;position2;position3;velocity1;velocity2;velocity3]. This vector is nothing but the position and speed of a single point of the arm(problably the joint connected to the hand) on a 3 dimetional space. If you want to describe the state of every single joint then you need to use this matrix: [position11 position21 position31; position12 position22 position32; position31 position32 position33; velocity11 velocity21 velocity31; velocity12 velocity22 velocity32; velocity31 velocity32 velocity33]. But usually you dont need this matrix because the state of the third joint is a function of the states of the first and second joints. You can use [position1;position2;position3;velocity1;velocity2;velocity3] for describing the state of a helicopter in the air or submarine in the sea. I just mentioned this to avoid confusion. Some readers might think that what you say applys only to robotic arms with 3 joints.

second: Your post is great but the math is difficult to read. I have already written a couple of mathematical posts in this forum but I regret it because it is relatively difficult to understand later(ASCII and UNICODE are not for that after all). If you plan to write a guide or tutorial with maths I would recomend posting PDFs or HTML(using the tutorial webspace that admin provide).

Finally MIT-OCW is full of extra stuff. For expample: just type kalman filtering and you will get everything you dreamend. It is a bit difficult to understand in the beggining since most of the documents are just notes for the classes, not real explanations. But still it is better than the wikipedia page.

If you want any help just pm me.

[sdk32285]

You by chance wouldn't be able to recommend a good book that covers Kalman Filters, and other control theory kind of stuff. I just had my yearly review and they said they would kick in for some high quality books, so I'm just kinda hunting around at the moment.

No I have been looking but I can not find any good books that I like. There is a book by Maybeck that people consider the first chapter to be the ultimate introduction to what a kalman filter is. You can find it online at http://www.cs.unc.edu/~welch/media/pdf/maybeck_ch1.pdf

I'm going to reiterate what Tsukubadaisei said:
This filter can be used for just about any system. My examples using a Robotic Arm, 3 links, positions, velocities, torques, etc.. is completely random.
Kalman filters are commonly used with just a single sensor, fusing multiple sensors, mobile robots, etc...

Also the reason that I am doing this is so that I can write it as I have time, while allowing people to see/comment on it. Once it's done I might compile it all together with the questions/comments to form a PDF. Part of my motivation to write this is that I have not been able to find any kalman tutorials that really explain it at a low level with out throwing in a lot of (arguably useless) math.

[ceruleanplains]

I use Statistical Orbit Determination by T.S.B. (three last names) for a good reference and intro to a varity of filters.  It teaches you how to derive the state transition matrix as well as some materials on your state equations in general.  Then it provides some excellent coverage of kalmann and batch techniques as well as some square root free matrix inversions.  It doesn't have much discussion about the weighting matrix for your measurements or the initial guess at your covariance, but that's an exercise in knowledge and experience with the applications anyway.

Square root matrix inversions are critical for things like robotics where you don't have that much processing to deal with those kinds of issues.  It's called a Cholesky Decomposition.  It's also a problem if your microprocessor can't do more than 16 bit operations you introduce some large errors rapidly.  I implemented these routines in Matlab (and FORTRAN). 

[sdk32285]

Continued from above.

The next variables we need to find are A and B. These matrices are the model that represents how your system changes from one state to the next.
Since the kalman filter is for linear systems we can assume that if start at the beginning (t=0) and you run your system to the next state(t=1), and then you run it again(t=2), the amount of change will be the same so we can use that change no matter where (t=wherever) in the system we actually are.
So the general idea is that you start somewhere and perturb your system and record the states, perturb again, record again, etc.

The B matrix can be made in the same way except that instead of perturbing the state we perturb the actions.

So in psudocode this would be:

X0 = state you are linearizing about;
U0 = torques required to produce (inverse dynamics);

delta = small number;
//Finding The A matrix
for ii=1 thru #ofStates{
X=X0;
X(ii)=X(ii)+delta;           //perturb state x(i) by delta
X1 = f(X,U0);         //f() is the model of system
A(:,ii) = (X1-X0)/delta;
}

//Finding The B matrix
for ii=1 thru #ofInputs {
U=U0;
U(ii)=U(ii)+delta;           //perturb action U(i) by delta
X1 = f(X0,U);         //f() is the model of system
B(:,ii) = (X1-X0)/delta;
}

The f() is your model. This model can take a state and an action and determine what the next state will be. This step involves the kinematics and dynamics of the rover.

For nonlinear system this is the step which gets changed to form an Extended Kalman Filter (EKF). For the EKF you need to linearize your model and than form your A and B matrices

[sdk32285]

Here is some c code for a kalman filter I designed for a PUMA 3DOF robotic arm (THIS IS A DIFFERENT SYSTEM THAN MY EXAMPLES ABOVE). I am using it for velocity estimation as this is much more accurate than just differentiating position.
I made bad assumptions for my noise model and sensor model to simplify the implementation. I also just initialize my covariance as an identity matrix. In my real code I let it converge and save it out to a text file than I read that text file every time I start the filter.

note: with the matrix library I use F*=G means F=F*G (and so forth with += and -=)

/*******************************************************/
/*********************************************
 * Kalman filter PUMA 3DOF Robotic Arm
 * 4/28/2008
 * By: sdk32285
 * posted at www.societyofrobots.com
 ********************************************/

#include <fstream>
#include <iostream>
#include <unistd.h>
#include <stdlib.h>
#include <string.h>
#include <stdio.h>
#include "matrixClass.h"  //my matrix library, you can use your own favorite matrix library

#define TIMESTEP 0.05

using namespace std;

float timeIndex,m_a1,m_a2,m_a3,c_a1,c_a2,c_a3,tau1,tau2,tau3;
float a1,a2,a3,a1d,a2d,a3d,a1dd,a2dd,a3dd;
float new_a1d, new_a2d, new_a3d;
float TIME=0;
matrix A(6,6);
matrix B(6,3);
matrix C=matrix::eye(6);  //initialize them as 6x6 identity matricies
matrix Q=matrix::eye(6);
matrix R=matrix::eye(6);
matrix y(6,1);
matrix K(6,6);
matrix x(6,1);
matrix state(6,1);
matrix action(3,1);
matrix lastState(6,1);
matrix P=matrix::eye(6);
matrix p=matrix::eye(6);
matrix measurment(6,1);

void initKalman(){

 float a[6][6]={
      {1.004,    0.0001,  0.001,    0.0014,    0.0000,  -0.0003  },
      {0.000,    1.000,     -0.00,      0.0000,    0.0019,   0          },
      {0.0004,  0.0002,  1.002,    0.0003,    0.0001,   0.0015   },
      {0.2028,  0.0481,  0.0433,  0.7114,   -0.0166,  -0.1458   },
      {0.0080,  0.0021, -0.0020, -0.0224,    0.9289,   0.005    },
      {0.1895,  0.1009,  0.1101, -0.1602,    0.0621,   0.7404  }
         };

 float b[6][3] = {
      {0.0000,   0.0000 ,  0.0000  },
      {0.0000,   0.0000,  -0.0000  },
      {0.0000,  -0.0000,   0.0000  },
      {0.007,    -0.0000,   0.0005 },
      {0.0001,   0.0000,  -0.0000 },
      {0.0003,  -0.0000,   0.0008 }
           };

/*loads the A and B matricies from above*/
 double value;
 for (int i = 0; i < 6; i++){
   for (int j = 0; j < 6; j++){
        value = a[j];
        A[j]=value;
    }
 }
 for (int i = 0; i < 6; i++){
   for (int j = 0; j < 3; j++){
   value = b[j];
        B[j]=value;
    }
 }

/*initializes the state*/
 state[0][0]=0.1;
 state[1][0]=0.1;
 state[2][0]=0.1;
 state[3][0]=0.1;
 state[4][0]=0.1;
 state[5][0]=0.1;

 lastState=state;

}

void kalman(){
 lastState=state;
 state[0][0]=c_a1;
 state[1][0]=c_a2;
 state[2][0]=c_a3;
 state[3][0]=a1d;
 state[4][0]=a2d;
 state[5][0]=a3d;
 
 measurment[0][0]=m_a1;
 measurment[1][0]=m_a2;
 measurment[2][0]=m_a3;
 measurment[3][0]=a1d;
 measurment[4][0]=a2d;
 measurment[5][0]=a3d;
 
 action[0][0]=tau1;
 action[1][0]=tau2;
 action[2][0]=tau3;
   
 matrix temp1(6,6);
 matrix temp2(6,6);
 matrix temp3(6,6);
 matrix temp4(6,1);
 /************Time Update*****************/
 //x = A*lastState + B*action;
 temp1=A;
 temp2=B;
 temp1*=lastState;
 temp2*=action;
 temp1+=temp2;
 x=temp1;

 //p = A*P*A' + Q;
 temp1=A;
 temp2=A;
 temp2.transpose();
 temp1*=P;
 temp1*=temp2;
 temp1+=Q;
 p=temp1;
 /************Measurement Update**********/
 //K = p*C*pinv(C*p*C'+R);
 temp1=C;
 temp2=p;
 temp1*=p;
 temp1*=C.transpose();
 temp1+=R;
 temp2*=C;
 temp2*=temp1;
 temp2=*pinv(temp1) //pinv is a function that takes the psudo inverse of a matix so for: pinv(F)=(F'*F)^-1 * F'
 K=temp2
 
 //y=C*state;
 temp1=C;
 temp1*=measurment;
 y=temp1;

 //state = x + K*(y-C*lastState);
 temp1=y;
 temp2=K;
 temp3=C;
 temp4=x;
 temp3*=lastState;
 temp1-=temp3;
 temp2*=temp1;
 temp4+=temp2;
 state=temp4;

 //P = (eye(6) - K*C)*p;
 temp1=K;
 temp2=C;
 temp1*=temp2;
 temp2-=temp1;
 temp2*=p;
 P=temp2;

 a1=state[0][0];
 a2=state[1][0];
 a3=state[2][0];
 a1d=state[3][0];
 a2d=state[4][0];
 a3d=state[5][0];
}

/* This function is not used since I am using position to get velocity (ie differentiation)
 * however I think that it is usefull to include since if you want velocity and position
 * from acceleration than you would use it */
void integrate(){
 new_a1d = a1d + a1dd*TIMESTEP;
 a1 += (new_a1d + a1d)*TIMESTEP/2;
 a1d = new_a1d;
 new_a2d = a2d + a2dd*TIMESTEP;
 a2 += (new_a2d + a2d)*TIMESTEP/2;
 a2d = new_a2d;
 new_a3d = a3d + a3dd*TIMESTEP;
 a3 += (new_a3d + a3d)*TIMESTEP/2;
 a3d = new_a3d;
 TIME+=TIMESTEP; 
}

/*This gives me velocity from position*/
void differentiation(){
a1d=(state[0][0]-lastState[0][0])/TIMESTEP;
a1d=(state[1][0]-lastState[1][0])/TIMESTEP;
a1d=(state[2][0]-lastState[2][0])/TIMESTEP;
TIME+=TIMESTEP;
}

int main () {

 initKalman();
 char buffer[500];
 ifstream readFile ("DATA.txt"); // this is where I read my data since I am proccessing it all offline

 while (!readFile.eof()){
   readFile.getline (buffer,500);
   sscanf(buffer, "%f %f %f %f %f %f %f %f %f %f ",&timeIndex,&m_a1,&m_a2,&m_a3,&c_a1,&c_a2,&c_a3,&tau1,&tau2,&tau3);
   
   kalman();
   differentiation();
   //integrate();

   /*this is where I log the results to and/or print it to the screen */
   FILE *file=fopen("filterOutput.txt", "a"); 
   fprintf(file,"%f %f %f %f %f %f %f %f %f %f \n",TIME,a1,a2,a3,a1d,a2d,a3d,tau1,tau2,tau3);
   fprintf(stderr,"%f %f %f %f %f %f %f %f %f %f \n",TIME,a1,a2,a3,a1d,a2d,a3d,tau1,tau2,tau3);
   fclose(file);
 }
 return 1;
}

[Tsukubadaisei]

Here is some c code for a kalman filter I designed for a PUMA 3DOF robotic arm ....

I think this is c++.

[sdk32285]

I think this is c++.

my bad, you are correct it is c++

[sdk32285]

Since for the Kalman Filter we assume that we are dealing with a linear system there is another way to find the A and B matrices.
If you are able to collect data from your system than you can solve A and B numerically.
So:
Since newState=A*lastState    then   A=newState * pinv(lastState)
pinv is the pseudoinverse of the matrix. So if lastState is not square than you can not normally take the inverse. The solution is to take the pseudoinverse which means you take (A' * A)^-1 * A'

Following this logic once you have A you can find B
So:
x=A*lastState + B*Action
Knowing the current state, what the last state was, A, and the action required to switch states you can solve for B

Note: On systems where you are just filtering stuff (i.e. sensors) than you might not be capable of having any actions so the B*action term cancels out and all you need to know is your A term.

[Luck768]

yes that is C++ .,I am an Information Technology graduate so I am sure that is  a C++ codes.,


 
_________________
Aprilaire