__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

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