Using an Extended Kalman Filter for Estimating Vehicle Dynamics and Mass

In this tutorial a slip control loop for a quarter car model is developed. Various of the vehicle states (such as longitudinal speed, wheel slip, road surface friction coefficient, and mass) are not directly measurable and hence must be estimated. An extended Kalman filter is implemented to perform the estimation based on a noisy measurement of wheel angular velocity.

This tutorial assumes that the reader is familiar with the basics of the quarter car model and the extended Kalman Filter. For a discussion of the dynamic equations of a quarter car model and implementing a slip controller assuming perfect state measurement see the Slip control of a Quarter Car Model tutorial. For a discussion of the mathematical background of the extended Kalman filter see theAn Introduction to the Extended Kalman Filter tutorial. Tutorials on general Simulink usage, Kalman filters, and their implementation in Simulink, can be found on theSoftware Tutorials page.

The tutorial is split into the following sections,

1. A Simulink Implementation.
2. Discretizing the Quarter Car Model.
3. Defining the EKF Equations.
4. Implementing the EKF.
5. Simulating The Model.

A Simulink Implementation

A Simulink model that implements a slip control loop using the extended Kalman filter developed in this tutorial is shown inFigure 1.

Figure 1: Simulink Model for Vehicle Slip Control using an Extended Kalman Filter.

The Simulink model of Figure 1 contains a continuous time quarter car model that is used to represent thereal physical vehicle and a discrete slip control loop. The slip control loop is comprised of the extended Kalman filter (developed in this tutorial) and a discrete PI controller (developed in theSlip Control of a Quarter Car Model tutorial).

A zip file containing the model of Figure 1 may be downloaded here.

Discretizing the Quarter Car Model

The standard quarter car model (as described in The Quarter Car Model) is a set of continuous time differential equations. However, the extended Kalman filter requires a set of discrete equations. Hence for use within an extended Kalman filter the quarter car model equations must be discretized.

The simplest approach for discretization is to use a forward Euler method. This lead to the discrete model

Equation 1: Discrete Quarter Car Equations.

where the subscript k indicates a particular discrete point in time andk-1 indicates the previous time point. Note that Equation 1 includes not only the vehicle dynamics but also the discretized tire model.

Defining the EKF Equations

The notation used in this section follows that used to describe the mathematics behind the extended Kalman filter in theExtended Kalman Filter tutorial. Of particular importance is the requirement to define the signals that can be measured (and hence act as inputs to the extended Kalman filter) and those states that need to be estimated (by the extended Kalman filter).

In automotive applications the wheel rotational speed ω can usually be measured (albeit corrupted by noise) and for this example it is assumed to be the only measurable entity. There are however five states that it has been determined need to be estimated:

• ω – the vehicle rotational speed (filtered from the noisy measurement).
• v – the vehicle longitudinal speed.
• λ – the tire slip.
• μx – the road friction coefficient.
• m – the (quarter) vehicle mass.

The measured input and state vector to be estimated are shown in Equation 2,

Equation 2: EKF Input and States to be Estimated.

Note that some of the states in Equation 2 have been chosen arbitrarily. The vehicle longitudinal speed is a typical state that requires estimating. However including the slip, friction coefficient and particularly the vehicle mass in the states to be estimated is purely a design decision. At first glance mass in particular may be considered an unusual parameter to estimate. However, if the vehicle in question is highly configurable (i.e. is capable of carry loads that range from very heavy to very light or non-existent) then a mass estimate may be important.

The extended Kalman filter uses a two step predictor–corrector algorithm. As part of the algorithm two Jacobians are required,Fk and Hk. For the discrete quarter car model ofEquation 1 with the measured input and states of Equation 2 these are,

Equation 3: Jacobians Used in the EKF.

The last row of Fk indicates that the mass of the vehicle is assumed to be constant. However, due to the stochastic nature of the extended Kalman filter, it really means that the mass is allowed to be slowly varying.

Implementing the EKF

The extended Kalman filter has been implemented using an Embedded MATLAB Function block. The block is discrete with a sample time of 5ms. The code for the block is shown below.

Note that the noise covariance matrices R and Q are based upon best guestimates. In practise their values would need to be tuned based on measured data.

function [vel,angVel,lambda,mu_x,mass] = ...
    ExtKalman(meas,input,Ts,v0,m0,Wr,g,J,roadCoeffs)
%#eml
% This Embedded MATLAB Function implements an extended Kalman filter used
% to estimate the states, road surface and mass of a quarter car model.
%
% The states of the process are given by
% x = [omega;velocity;slip;road friction;mass];
%
% There is assumed to be one measurement
% y = [omega]
%

% Author: Phil Goddard ([email protected])

% Define storage for the variables that need to persist
% between time periods.
persistent P xhat Q R
if isempty(P)
   % First time through the code so do some initialization
   xhat = [v0/Wr;v0;0;0;m0];
   P = zeros(5,5);
   Q = diag([0.01^2 0.01^2 1 1 0.001^2]);
   R = 0.005/Ts;
end
% Calculate the Jacobians for the state and measurement equations
F = [1 0 0 Ts*(1/J)*(Wr*g*xhat(5)) Ts*(1/J)*(Wr*g*xhat(4));
     0 1 0 Ts*(-g) 0;
     -(Wr/xhat(2)) Wr*xhat(1)/(xhat(2)^2) 0 0 0;
     0 0 roadCoeffs(1)*...
     (roadCoeffs(2)*exp(-roadCoeffs(2)*xhat(3))-roadCoeffs(3)) 0 0;
     0 0 0 0 1];
H = [1 0 0 0 0];
% Propogate the state and covariance matrices
xhat = [...
    xhat(1) + Ts*(1/J)*(Wr*g*xhat(4)*xhat(5) - sign(xhat(1))*input);
    xhat(2) + Ts*(-g*xhat(4));
    (xhat(2) - Wr*xhat(1))/xhat(2);
    roadCoeffs(1)*(1 - exp(-roadCoeffs(2)*xhat(3))-roadCoeffs(3)*xhat(3));
    xhat(5)];
P = F*P*F' + Q;
% Calculate the Kalman gain
K = P*H'/(H*P*H' + R);
% Calculate the measurement residual
yhat = xhat(1);
resid = meas - yhat;
% Update the state and covariance estimates
xhat = xhat + K*resid;
P = (eye(size(K,1))-K*H)*P;
% Post the results
vel = xhat(2);
angVel = xhat(1);
lambda = xhat(3);
mu_x = xhat(4);
mass = xhat(5);
      

Figure 2: M-Code for EKF Implemented in an Embedded MATLAB Function Block.

Simulating The Model

The results of simulating the model of Figure 1 with the extended Kalman filter of Figure 2 are shown below. Figure 3 shows the signals output by the real continuous time vehicle, whileFigure 4 shows the signals estimated by the extended Kalman filter. It is these estimated signals that are fed back to the controller to form the slip control loop.

Figure 3: Actual v and ω (Top) and λ (Bottom).

Figure 4: Estimated v and ω (Top) and λ (Bottom).

Note that the estimated slip is quite noisy. This isn’t untypical of real slip data, however in the simulation it could possibly be reduced by better tuning of the noise covariance matricesR and Q.

Figure 5: Estimated Vehicle Mass.

A final comment should be made regarding the vehicle mass estimate. Figure 5 shows a plot of the estimated mass. It is initialized to 450Kg which is the same as the actual vehicle mass and it does not substantially change. Over the 3s of this simulation the mass shouldn’t be expected to change. Only over a longer time frame, where the model is designed to take account of fuel use or variable loads, would a change be expected to be noticed.

This tutorial has discussed the implementation in Simulink of an extended Kalman filter for estimating various states of an automotive quarter car model and using them as part of a slip control loop. Other Simulink tutorials are available on theSoftware Tutorials page