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