## Wednesday, February 29, 2012

### Kalman Filtering for Dummies - Part VI

In this post, I will be discussing about Kalman filtering for dynamic models. The Tustin's transform will be used to transform a continuous state space to a discrete equivalent, then a Matlab code will be given to show the simulation of the Kalman filter. The model I am using the a armature controlled - permanent magnet - brush motor.

Discretization of Continuous State Space
There are several method to discretize a state space but I found it the Tustin's transform is the most easily understood and more accurate compared to Euler's transform.

Example (Permanent magnet direct current motor model):

Above is the state equation for a dc motor. Reader can find the explanation of this model from Google.

By expressing the state equation in vector and matrix (compact form), we obtain the above equations. Also by applying La Place transform with zero initial values, we can get the frequency domain equivalent.

Approximate the state equation in the frequency domain with the Tustin transformation equation, we ca actually discretize the state equation. On other words, we obtain the discrete state space equation.

By applying the Tustin transform and several algebraic manipulation, the discrete state space equation above is obtain. As shown in the equation, the current state variable value are approximated with the previous state equation with the state transition matrix and the input.

The notation were simplified to quite the Kalman filter algorithm.

Kalman filter algorithm DC Motor

Ra = 2.06;
La = 0.000238;
Kt = 0.0235;
Ke = 1/((402*2*pi)/60);
Jm = 1.07e-6 * 2;
Bm = 12e-7 * 2;

A = [0 1 0; 0 -Bm/Jm Kt/Jm; 0 -Ke/La -Ra/La];
B = [0; 0; 1/La];
C = [1 0 0; 0 1 0];
D = 0;

Ts = 0.00005;

F = (eye(3) - A * Ts / 2)\(eye(3) + A * Ts / 2);
G = (eye(3) - A * Ts / 2)\(B * Ts);

time = 0: Ts : 0.4;
va_amp = 5;

%Sine input
va = va_amp * sin (2* 5 * pi * time);

qk(:,1) = [0; 0; 0];

for n = 1:length(time)-1
qk(:, n+1) = F * qk(:, n) + G * (va(n+1) + va(n))/2;
end

qk_noise = qk + randn(size(qk,1),size(qk,2))*0.05;

%Kalman filter initialization
qk_est = [0; 0; 0];    %initialize the states
P = eye(3);        %initialize the estimate convariance

%F and H are the model parameters where
H = [1 0 0; 0 1 0];

%Q and R both the process and measurement noise covariances
Q = F*F'* 0.01;
R = [1 0 ; 0 1];

%Kalman iterate
for n = 1:length(time)-1
qk_est(:,n+1) = F * qk_est(:,n) + G * (va(n+1) + va(n))/2;
P=F*P*transpose(F) + Q;

z=[qk_noise(1, n+1);qk_noise(2, n+1)];
y=z-H*qk_est(:,n+1);
S=H*P*transpose(H)+R;
K=P*transpose(H)/S;
qk_est(:,n+1) = qk_est(:,n+1) + K*y;
P=(eye(3)-K*H)*P;
fprintf('Kalman iteration = %i\n', length(time) - n);
end

The plot above is for a dc motor system with position and velocity measurements. The Kalman filter is not only used to fuse the data but to estimate the armature current in the motor.

Wrap Up
- Kalman filter is used to fuse data from several sensors measurements
- It is also can be used to estimate internal state
- To start using this filter, the system model have to be identified first.
- The model can be of any type, kinematic or dynamic, with input or inputless. But using Kalman filter requires a discrete state space model.
- Several examples have been shown, for example a position-velocity-acceleration system, getting a pitch from accelerometer and gyroscope, getting the heading from a mobile robot, and dc motor.

Part I
Part II
Part III
Part IV
Part V
Part VI

## Friday, February 24, 2012

### Kalman Filtering for Dummies - Part V

Two example will be given in this post. Note that all the examples are discrete state space in kinematic form. And all these systems are inputless.

Getting pitch from Accelerometer and Rate Gyroscope

The absolute pitch can be obtain from the accelerometers. Due to the effect of gravity on the ay and az direction, the absolute pitch can be obtained from atan2 function of ay and az value. But sometimes the absolute pitch is effected by noise from actual acceleration and deceleration. Therefore this is when the rate gyroscope comes in handy.

Therefore, we can design the state variables to be absolute pitch and rate of pitch. The state transition matrix is a simple kinematic matrix like previous parts. And since we can measure all the state variables, the output matrix is a size two identity matrix. The state equation and the output equation are as follow:

Getting the Robot Posture from Encoders, Accelerometer and Rate Gyroscope

There are several methods (model) to obtain the robot posture (x, y, and theta) from all the mentioned sensors. In this case, I would propose the reading from the rate gyroscope, accelerometer, and encoder with the sensors as placed, shown in figure above.

The problem is quite tricky because obtaining robot posture from encoder it not direct but via integration method. Besides, the robot posture information obtained is relative. In other words, the data obtain is not absolute; which implies that the data is prone to cumulative error. Kalman filter might help to reduce the cumulative error but will not solve it unless there is a sensor to obtain absolute position reading (i.e. GPS).

To solve this problem, I choose to use four state variables. The longitudinal velocity, longitudinal acceleration, ax, heading and heading rate, w. This will be a four dimensional Kalman filter. But since the longitudinal velocity and acceleration are orthonormal with heading and heading rate, the Kalman filter can be decoupled in to two separate Kalman filter. I will be sticking to the four dimensional Kalman.

Since all states variables can be measured, the output matrix is a size four identity matrix. The heading, theta can be obtain from integration from the encoders, heading rate, from the rate heading gyroscope, longitudinal velocity, vx, from the encoders and finally, the longitudinal acceleration from the accelerometer.

The robot posture is not obtained directly from the state variables but through some kind of matrix. Users can search for the kinematic equation for a robot posture on the internet.

In the next part, I will be showing how to obtain Kalman state equation and output equation for a dynamic system. This system will have an input, as opposed to the examples shown until now. Kalman filter in a dynamic system can be used together with linear quadratic regulator to solve a linear quadratic gaussian problem. This is a type of optimal controller in advance control system study.

Part I
Part II
Part III
Part IV
Part V
Part VI

### Kalman Filtering for Dummies - Part IV

Here goes the coding in Matlab for the position-velocity-acceleration system:

First Example, full observability (all sensors are available).

clear all
clc

%timeline and sampling period, Ts
Ts = 0.01;
t = 0:Ts:10;

%true parameters position, velocity and acceleration
x = cos(t);
dx = -sin(t);
ddx = -cos(t);

for i=1:length(t)
x_noise(i) = x(i) + (randn());
dx_noise(i) = dx(i) + (randn());
ddx_noise(i) = ddx(i) + (randn());
end

%Kalman filter initialization
%initialize the states, here, the initial value is incorrect
q = [0; 0; 1];
%initialize the estimate covariance with nonzero values.
P = eye(3);

%F and H are the model parameters where
F = [1 Ts 0.5*Ts*Ts; 0 1 Ts; 0 0 1];
H = [1 0 0; 0 1 0; 0 0 1];

%Q and R both the process and measurement noise covariances
Q = [ 0 0 0; 0 0 0; 0 0 1];
R = [1 0 0; 0 1 0; 0 0 1];

%Kalman iterate
for i = 1:length(t)-1
%predict stage
q(:,i+1) = F * q(:,i);
P=F*P*transpose(F) + Q;

%this is where the measurement is corrupted with noise
z=[x_noise(i);dx_noise(i);ddx_noise(i)];

%update stage
y=z-H*q(:,i+1);
S=H*P*transpose(H)+R;
K=P*transpose(H)/S;
q(:,i+1) = q(:,i+1) + K*y;
P=(eye(3)-K*H)*P;
end

%Plotting the data
subplot(311)
my_plot = plot(t,x,t,q(1,:),t,x_noise,':');
set(my_plot, 'linewidth', 1);
legend('Position','Estimate Position','Position with Noise');
subplot(312)
my_plot = plot(t,dx,t,q(2,:),t, dx_noise,':');
set(my_plot, 'linewidth', 1);
legend('Velocity','Estimate Velocity','Velocity with Noise');
subplot(313)
my_plot = plot(t,ddx,t,q(3,:),t,ddx_noise,':');
set(my_plot, 'linewidth', 1);
legend('Acceleration','Estimate Acceleration','Acceleration with noise');
 Full observer
The picture above is plotted from the Matlab file. As seen, the position and velocity reading is quite clean (green line) despite the corrupted measurement (red line) as compared to the true value (blue line). As for the acceleration, the data is not really clean because the model used does not fuse well for acceleration.

Second example, only position and velocity sensor:

Ts = 0.01;
t = 0:Ts:10;

x = cos(t);
dx = -sin(t);
ddx = -cos(t);

for i=1:length(t)
x_noise(i) = x(i) + (randn());
dx_noise(i) = dx(i) + (randn());
ddx_noise(i) = ddx(i) + (randn());
end

Ts = 0.01;
q = [1; 0; -1];
P=zeros(3);

%As seen here, the H matrix is different from first example.
H = [1 0 0; 0 1 0];
F = [1 Ts 0.5*Ts*Ts; 0 1 Ts; 0 0 1];

%process covariance
Q = [0.0001 0 0; 0 0.0001 0; 0 0 0.1];
%measurement covariance

R = [1 0 ; 0 1];

%Kalman filter interation
for i = 1:length(t)-1
q(:,i+1) = F * q(:,i);
P=F*P*transpose(F) + Q;

%The measurement only have two sensors
z=[x_noise(i);dx_noise(i)];

y=z-H*q(:,i+1);
S=H*P*transpose(H)+R;
K=P*transpose(H)/S;
q(:,i+1) = q(:,i+1) + K*y;
P=(eye(3)-K*H)*P;
end

 Position and velocity observer
As seen in the plot above, only the position have significantly clean data. There is a reduction in noise in the velocity measurement but the acceleration is totally noisy and unpredictable.

Third example, only velocity and acceleration sensor:

Ts = 0.01;
t = 0:Ts:10;

x = cos(t);
dx = -sin(t);
ddx = -cos(t);

for i=1:length(t)
x_noise(i) = x(i) + (randn());
dx_noise(i) = dx(i) + (randn());
ddx_noise(i) = ddx(i) + (randn());
end

q = [0; 0; 1];
P=eye(3);

%The H matrix is different from first two examples
H = [0 1 0; 0 0 1];
F = [1 Ts 0.5*Ts*Ts; 0 1 Ts; 0 0 1];

%As seen from all example, the Q can take on any value and still works fine.
Q = [0.25*power(Ts,4) 0.5*power(Ts,3) 0.5*Ts;
0.5*power(Ts,3) power(Ts,2) Ts;
0.5*power(Ts,2) Ts 1];
R = [1 0 ; 0 1 ];

for i = 1:length(t)-1
q(:,i+1) = F * q(:,i);
P=F*P*transpose(F) + Q;

%Only velocity and acceleration measurement
z=[dx_noise(i);ddx_noise(i)];

y=z-H*q(:,i+1);
S=H*P*transpose(H)+R;
K=P*transpose(H)/S;
q(:,i+1) = q(:,i+1) + K*y;
P=(eye(3)-K*H)*P;
end

 Velocity and acceleration sensors
From picture above, only velocity gives clean data while the position sensor is off. This is due to the lack of position sensor, and this can be avoided if the position initial value is exactly known.

In a nutshell:
From these three examples, all the F matrix is the same because the internal state variables are interconnected similarly. The H is different because of the usage of different sensor sets.

In part V, I will show other examples where Kalman filter can be used, including an example to obtain the pitch from accelerometer and gyroscope and an example for sensors in navigation.

Part I
Part II
Part III
Part IV
Part V
Part VI

## Thursday, February 23, 2012

### Kalman Filtering for Dummies - Part III

Let's do some recap.

q[k] = F * q[k-1] --- Equation -1
w[k] = H * q[k] --- Equation 0

Equation -1 and 0 are both the state equation and output equation, where the state variable is q, outputs is w, F is the state transition matrix and H is the output matrix. I have changed the notation from previous part so to not confuse with the Kalman formula below.

Following on, the Kalman formula will be applied. The formulas given below are adapted from Wikipedia where I have modified some notation.

Before that, I would like to explain about:

k-1|k-1: This is the previous filtered sample data.
k|k-1: This is the a priori prediction for the Kalman filter. Simply put that this is the predicted variables.
k|k: This is the a posteriori update for the Kalman filter. This is the updated variables, means filtered data.
 Predict

From Equation 1 and 2, we have to predict the state variables, q, and the state variables covariance, P. Note that if for sampling time is 0, q0|0 can take any value, while P0|0 can be a zero matrix if the initial value for state is known or any diagonal valued matrix if the initial value is unknown. Q is the process covariance or the prediction covariance. Put differently, this is the variance that will occur during the prediction stage.

Obtaining the value of Q is not practical because the process is quite hidden. But I would use F * transpose (F). I do not guarantee the value to be correct but I used this without any major problem. Sometimes, it does depend on intuition too.
 Update

From Equation 3, z is the measured data. For example, from previous part, the measured data is position, velocity and acceleration. H * x is the output equation from the predicted state variable from Equation 1. y is the residual from the measured data and the predicted data.

From Equation 4 and 5, these are just the calculation of Kalman gain from various covariances. This is also means the calculation based on the degree of trust, which later used to update the predicted state variables. R is the measurement covariance where sensors do not always give correct data. From our example of the positional - velocity - acceleration system, since all parameters are independent of each other, the covariance is just a diagonal matrix. Where the first element in the diagonal reflects the variance in the position measurement, second element the velocity measurement and the third diagonal element, the variance of the acceleration measurement.

Obtaining the measurement covariance is also quite impractical, unless there is another accurate sensor to get the variance of each sensor. Sometimes the tuning of R will depend on intuition.

Equation 6 is actually the correction done to the state variable from the predicted state variable by adding the product of Kalman gain and the residual. Equation 7 is the update of the state variable covariance.

In part IV, I will give a coding example in Matlab for the position, velocity and acceleration system. I will also plot the data to show how effective the Kalman filter algorithm is, despite the presence of noise.

Part I
Part II
Part III
Part IV
Part V
Part VI

### Kalman Filtering for Dummies Part II

Using Kalman filter is all about the underlying model. Specifically discrete state space model. The continuous counterpart of Kalman filter is called the Kalman-Bucy filter, but it would be meaningless because Kalman filter is usually implemented in a digital controller. For simplicity, I would be using examples for kinematic models which are more easily understood as opposed to a dynamic model. A discrete dynamic model can be derived from first principle, applying La Place transform and discretized, to form a discrete state space. But for kinematic model, it can be easily derived from basic kinematic equation.

Using Kalman filter deals with a lot of matrices and vectors. Therefore it is better to have good basics in linear system. We shall start with a simple example.

x[n] = x[n-1] + T * v[n-1] + 0.5 * T * T * a[n-1] --- Equation 1
v[n] = v[n-1] + T * a[n-1] --- Equation 2
a[n] = a[n-1] --- Equation 3

Explanation:
- For equation 1, 2, and 3, I used the square bracket to denote sample. [n] is the value of state in the current sample, [n-1] is the value in the previous sample while [n-k] is the value is the previous k sample.
- x is the position state, v is the velocity state while a is the acceleration state. T is the sampling time, that is the period of one sample time to another sample.
- Equation 1 simply means I would like to predict the current position value from the previous sample using simple kinematic equations. So as Equation 2. As for Equation 3, I can assume that he acceleration is constant throughout the time, but of course this is not true. This is when Kalman filter comes in handy.

Arranging Equation 1, 2, and 3 in matrix form:
where q, the state vector is the vector containing three state variables, position, velocity and acceleration, and F is the state transition matrix. The whole equation is called the state equation (as in discrete state space kinematic model). Note that state equation is internal and hidden from the user. Put differently, the value is not known unless measured. If you are familiar with state space control, this model is also an inputless system.

Next, we would deal with the output equation. Output equation is the equation dealing with the the state that we are going to measure. For this example, we are going to measure all the state variables. Therefore the equation is simply an identity matrix. In other words, each state variables will have their own dedicated sensors.

Rewriting the output equation in matrix form, H is identity matrix. There is also cases where not all states variables can be measure. This case shall be deal later, but in this case, H matrix is not identity.

To sum up, discrete state equation is the equation connecting the current state from previous state through a state transition matrix while the output equation is the connecting the measured variables and the state equation.

This is a simple example of a model where Kalman filter can be used. There are many other model where Kalman filter can be used, therefore it depends on how the model was derived and used along with this filter. Also in this example, the model is a linear model where F and H matrix is constant. There are also nonlinear models where F and H is not linear but dependent on the state variables or the derivatives, or integrals, or even time. This is when Extended Kalman Filter or Unscented Kalman Filter can be a good choice; which I will not cover this due to limited knowledge.

As for the next part, I will be discussing the implementation of the Kalman filter formula and brief explanation on the formula.

Part I
Part II
Part III
Part IV
Part V
Part VI

### Kalman Filtering for Dummies Part I

I find this topic quite interesting yet challenging. The application from this filter is vast and this filter itself is very useful in many data fusion application. As for the number of applications, the sky is the limit but from my background of study, I would like to discuss about this filter from robotics application point of view.

To start off, understanding this filter is not an easy task, due to the nature of the subject. Of course certain pre-requisites are required, for example linear system, control system engineering, state space control, discrete state space, etc. Note that this post is targeted to amateurs in robotics who wants to use this filter but have no strong backgrounds in the fields I have mentioned. As this post is targetted on the implementation of Kalman filter, there is no explanation on the proof or the underlying theory.

Before going further, I would like to share my experience in learning this filter. I find it hard to search for a decent Kalman filter tutorial, that is suitable for amateur roboticist. Most will just jump to the formula without explaining the theory behind, while some would just explain the theory with those rigorous mathematics. I will try my best to make this post comfortable for the "dummies". Hey, I used to be a dummy too.

Kalman filter is a very powerful filter in a sense that it can be used to fuse two related data to obtain a clean and more accurate data. For example in robotics application, I have a positional sensor (incremental encoder) and speed sensor (tachometer). Of course I can differentiate the position sensor to obtain the speed or integrate the speed sensor to obtain the position. But why do I need both sensors?

As a brief explanation, we could put that each sensors have their own variances. In other words, each sensor will not always give the correct reading but with error in a fashion that the error has a variance. Some sensors might have bigger variance than the other. Trusting only one sensor will put the measurement in a larger range of error. Kalman filter can be used to fuse these two sensors according to their variances, thus producing a smaller margin of error. It can be said that Kalman filter is a statistical filter or stochastic filter.

Besides that, Kalman filter can be used as observer in a system. An observer is a method or algorithm to predict the internal state of a system. For example in a positional - speed - acceleration system, we would need to have three separate sensors to get all three measurements. Trusting only positional sensors to get the speed and acceleration will puts the reading in large margin of error. Therefore Kalman filter can be used to fuse all three sensors according to the variances to obtain readings with smaller margin of error.

But more sensors means higher cost. Therefore, Kalman filter also can be used to observe the internal state that cannot be measured. For example, in the positional - speed - acceleration system, I would only need position and acceleration sensors to obtain the speed of the system, fused statistically. In other words, I can use one sensor to obtain reading of other internal state (that cannot be measured), in a manner that the reading is more accurate. You can Google for an observer called the Luenberger observer, and it can be observed that Kalman observer is more superior than the Luenberger observer.

I think this is too long a post. I will continue on Kalman filter in the next part. In the next part, I would explain on modeling and why it is important to have a model before applying Kalman filter. You might want to read this before hand.

Part I
Part II
Part III
Part IV
Part V
Part VI