4.1.1 Problem

You want to estimate the velocity and position of a mass attached through a spring and damper to a structure. The system is shown in Figure 4.1. m is the mass, k is the spring constant, c is the damping constant, and f is an external force. x is the position. The mass moves in only one direction.

Suppose we had a camera that was located near the mass. The camera would be pointed at the mass during its ascent. This would result in a measurement of the angle between the ground and the boresight of the camera. The angle measurement geometry is shown in Figure 4.2. The angle is measured from an offset baseline.

We want to use a conventional linear Kalman Filter to estimate the state of the system. This is suitable for a simple system that can be modeled with linear equations.

4.1.2 Solution

First, we will need to define a mathematical model for the mass system and code it up. Then we will derive the Kalman Filter from first principles, using Bayes theorem. Finally, we present code implementing the Kalman Filter estimator for the spring-mass problem.

4.1.3 How It Works

Spring-Mass System Model

The continuous time differential equations modeling the system are

(4.3)

(4.4)

This says the change in position r with respect to time t is the velocity v. The change in velocity with respect to time (times mass) is an external force, minus the damping constant times velocity, minus the spring constant times the position. The second equation is just Newton’s law where the total force is F and the total acceleration, a_T, is the total force divided by the mass,

(4.5)

(4.6)

To simplify the problem we divide both sides of the second equation by mass and get:

(4.7)

(4.8)

where

(4.9)

(4.10)

a is the acceleration due to external forces , ζ is the damping ratio, and ω is the undamped natural frequency. The undamped natural frequency is the frequency at which the mass would oscillate if there were no damping. The damping ratio indicates how fast the system damps and what level of oscillations we observe. With a damping ratio of zero, the system never damps and the mass oscillates forever. With a damping ratio of one you don’t see any oscillation. This form makes it easier to understand what damping and oscillation to expect. You immediately know the frequency and the rate at which the oscillation should subside. m, c, and k, although they embody the same information, don’t make this as obvious.

The following shows a simulation of the oscillator with damping (OscillatorDamping RatioSim). It shows different damping ratios. The loop that runs the simulation with different damping ratios is shown.

 for j = 1:length(zeta)

    % Initial state [position;velocity]

   x = [0;1];

    % Select damping ratio from array

   d.zeta= zeta(j);

    % Print a string for the legend

   s{j} =  sprintf ( ’zeta␣=␣%6.4f’,zeta(j));

    for k = 1:nSim

      % Plot storage

     xPlot(j,k)  = x(1);

      % Propagate (numerically integrate) the state equations

     x  = RungeKutta( @RHSOscillator, 0, x, dT, d );

    end

 end  

The results of the damping ratio demo are shown in Figure 4.3. The initial conditions are zero position and a velocity of one. The responses to different levels of damping ratios are seen. When zeta is zero it is undamped and oscillates forever. Critical damping, which is desirable from minimizing actuator effort, is 0.7071. A damping ratio of 1 results in no overshoot to a step disturbance. In this case, we have “overshoot,” since we are not at a rest initial condition.

The dynamical equations are in what is called state-space form because the derivative of the state vector:

(4.11)

has nothing multiplying it and there are only first derivatives on the left-hand side. Sometimes you see equations like:

(4.12)

If Q is not invertible then you can’t do:

(4.13)

to make state space equations. Conceptually, if Q is not invertible, that is the same thing as having fewer than N unique equations (where N is the length of x, the number of states).

All of our filter derivations work with dynamical equations in state space form. Also, most numerical integration schemes are designed for sets of first-order differential equations.

The right-hand side for the state equations (first-order differential equations), RHSOscillator, is shown in the following listing. Notice that if no inputs are requested, it returns the default data structure. The code, if ( nargin <1), tells the function to return the data structure if no inputs are given. This is a convenient way of making your functions self-documenting and keeping your data structures consistent. The actual working code is just one line.

 xDot = [x(2);d.a-2*d.zeta*d.omega*x(2)-d.omega^2*x(1)];  

The following listing gives the simulation script OscillatorSim. It causes the right-hand side, RHSOscillator, to be numerically integrated using the RungeKutta function. We start by getting the default data structure from the right-hand side. We fill it in with our desired parameters. Measurements y are created for each step, including random noise. There are two measurements: position and angle.

The following code shows just the simulation loop of OscillatorSim. The angle measurement is just trigonometry. The first measurement line computes the angle, which is a nonlinear measurement. The second measures the vertical distance, which is linear.

 for k = 1:nSim

    % Measurements

   yTheta =  atan (x(1)/baseline) + yTheta1Sigma* randn (1,1);

   yR     = x(1) + yR1Sigma* randn (1,1);

    % Plot storage

   xPlot(:,k) = [x;yTheta;yR];

    % Propagate (numerically integrate) the state equations

   x = RungeKutta( @RHSOscillator, 0, x, dT, dRHS );

 end  

The results of the simulation are shown in Figure 4.4. The input is a disturbance acceleration that goes from zero to its value at time t = 0. It is constant for the duration of the simulation. This is known as a step disturbance. This causes the system to oscillate. The magnitude of the oscillation slowly goes to zero because of the damping. If the damping ratio were 1, we would not see any oscillation, as seen in Figure 4.3.

The offset seen in the plot of r can be found analytically by setting v = 0. Essentially, the spring force is balancing the external force.

(4.14)

(4.15)

We have now completed the derivation of our model and can move on to building the Kalman Filters.

Kalman Filter Derivation

Kalman filters can be derived from Bayes’ Theorem. What is Bayes’ Theorem? Bayes’ Theorem is:

(4.16)

(4.17)

which is just the probability of A_i given B. P means “probability.” The vertical bar | means “given.” This assumes that the probability of B is not zero, that is, P(B)≠0. In the Bayesian interpretation, the theorem introduces the effect of evidence on belief. This provides a rigorous framework for incorporating any data for which there is a degree of uncertainty. Put simply, given all evidence (or data) to date, Bayes’ Theorem allows you to determine how new evidence affects the belief. In the case of state estimation this is the belief in the accuracy of the state estimate.

Figure 4.5 shows the Kalman Filter family and how it relates to the Bayesian Filter. In this book we are covering only the ones in the colored boxes. The complete derivation of the Kalman Filter is given below; this provides a coherent framework for all Kalman filtering implementations. The different filters fall out of the Bayesian models based on assumptions about the model and sensor noise and the linearity or nonlinearity of the measurement and dynamics models. Let’s look at the branch that is colored blue. Additive Gaussian noise filters can be linear or nonlinear depending on the type of dynamical and measurement models. In many cases you can take a nonlinear system and linearize it about the normal operating conditions. You can then use a linear Kalman Filter. For example, a spacecraft dynamical model is nonlinear and an Earth sensor that measures the Earth’s chord width for roll and pitch information is nonlinear. However, if we are only concerned with Earth pointing, and small deviations from nominal pointing, we can linearize both the dynamical equations and the measurement equations and use a linear Kalman Filter.

If nonlinearities are important, we have to use a nonlinear filter. The Extended Kalman Filter (EKF) uses partial derivatives of the measurement and dynamical equations. These are computed each time step or with each measurement input. In effect, we are linearizing the system each step and using the linear equations. We don’t have to do a linear state propagation, that is, propagating the dynamical equations, and could propagate them using numerical integration. If we can get analytical derivatives of the measurement and dynamical equations, this is a reasonable approach. If there are singularities in any of the equations, this may not work.

The Unscented Kalman Filter (UKF) uses the nonlinear equations directly. There are two forms, augmented and non-augmented. In the former, we created an augmented state vector that includes both the states and the state and measurement noise variables. This may result in better results at the expense of more computation.

All of the filters in this chapter are Markov, that is, the current dynamical state is entirely determined from the previous state. Particle filters are not addressed in this book. They are a class of Monte Carlo methods. Monte Carlo (named after the famous casino) methods are computational algorithms that rely on random sampling to obtain results. For example, a Monte Carlo approach to our oscillator simulation would be to use the MATLAB function nrandn to generate the accelerations. We’d run many tests to verify that our mass moves as expected.

Our derivation will use the notation N(μ, σ ²) to represent a normal variable. A normal variable is another word for a Gaussian variable. Gaussian means it is distributed as the normal distribution with mean μ (average) and variance σ ². The following code from Gaussian computes a Gaussian or Normal distribution around a mean of 2 for a range of standard deviations. Figure 4.6 shows a plot. The height of the plot indicates how likely a given measurement of the variable is to have that value.

  %% Initialize

 mu            = 2;            % Mean

 sigma         = [1 2 3 4];  % Standard deviation

 n             =  length (sigma);

 x             =  linspace (-7,10);

  %% Simulation

 xPlot =  zeros (n, length (x));

 s     =  cell (1,n);

 for k = 1:length(sigma)

   s{k}       =  sprintf ( ’Sigma␣=␣%3.1f’,sigma(k));

   f          = -(x-mu).^2/(2*sigma(k)^2);

   xPlot(k,:) =  exp (f)/ sqrt (2* pi *sigma(k)^2);

 end  

Given the probabilistic state space model in discrete time [23]

(4.18)

where x is the state vector and w is the noise vector, the measurement equation is:

(4.19)

where v_n is the measurement noise. This has the form of a hidden Markov model (HMM) because the state is hidden.

If the process is Markovian, then the future state x_k is dependent only on the current state x_k−1 and is not dependent on the past states. This can be expressed in the equation:

(4.20)

The | means given. In this case, the first term is read as “the probability of x_k given x _1:k−1 and y _1:k−1.” This is the probability of the current state given all past states and all measurements up to the k − 1 measurement. The past, x_k−1, is independent of the future given the present.

(4.21)

where T is the last sample and the measurements y_k are conditionally independent given x_k; that is, they can be determined using only x_k and are not dependent on x _1:k or y _1:k−1. This can be expressed as:

(4.22)

We can define the recursive Bayesian optimal filter that computes the distribution:

(4.23)

given:

• The prior distribution p(x ₀), where x ₀ is the state prior to the first measurement,

• The state space model

  

  (4.24)

  

  (4.25)

• The measurement sequence y _1:k = y ₁, …, y_k.

Computation is based on the recursion rule

(4.26)

This means that we get the current state x_k from the prior state x_k−1 and all the past measurements y _1:k−1. Assume that we know the posterior distribution of the previous time step:

(4.27)

The joint distribution of x_k, x_k−1 given y _1:k−1 can be computed as:

(4.28)

(4.29)

because this is a Markov process. Integrating over x_k−1 gives the prediction step of the optimal filter, which is the Chapman–Kolmogorov equation

(4.30)

The Chapman–Kolmogorov equation is an identity relating the joint probability distributions of different sets of coordinates on a stochastic process. The measurement update state is found from Bayes’ Rule:

(4.31)

(4.32)

C_k is the probability of the current measurement, given all past measurements.

If the noise is additive and Gaussian with the state covariance Q_n and the measurement covariance R_n, the model and measurement noise have zero mean, and we can write the state equation as:

(4.33)

where x is the state vector and w is the noise vector. The measurement equation becomes:

(4.34)

Given that Q is not time-dependent we can write:

(4.35)

where recall that N is a normal variable, in this case, with mean x_k;f(x_k−1), which means (x_k given f(x_k−1) and variance Q. We can now write the prediction step Equation 4.30 as:

(4.36)

We need to find the first two moments of x_k. A moment is the expected value (or mean) of the variable. The first moment is of the variable, the second is of the variable squared and so forth. They are:

(4.37)

(4.38)

E means expected value. E[x_k] is the mean and is the covariance. Expanding the first moment and using the identity where s is any argument.

(4.39)

(4.40)

(4.41)

Assuming that where P^xx is the covariance of x and noting that x_k = f_k(x_k−1) + w_k−1 we get:

(4.42)

For the second moment:

(4.43)

(4.44)

which results in:

(4.45)

The covariance for the initial state is Gaussian and is . The Kalman Filter can be written without further approximations as

(4.46)

(4.47)

(4.48)

where K_n is the Kalman gain and P^yy is the measurement covariance. The solution of these equations requires the solution of five integrals of the form:

(4.49)

The three integrals needed by the filter are:

(4.50)

(4.51)

(4.52)

Assume that we have a model of the form:

(4.53)

(4.54)

where

• is the state of system at time k

• m_k is the mean state at time k

• A_k−1 is the state transition matrix at time k − 1

• Bk − 1 is the input matrix at time k − 1

• uk − 1 is the input at time k − 1

• q_k−1, N(0, Q_k), is the Gaussian process noise at time k − 1

• is the measurement at time k

• H_k is the measurement matrix at time k. This is found from the Jacobian (derivatives) of h(x).

• r_k = N(0, R_k), is the Gaussian measurement noise at time k

• The prior distribution of the state is x ₀ = N(m ₀, P ₀) where parameters m ₀ and P ₀ contain all prior knowledge about the system. m ₀ is the mean at time zero and P ₀ is the covariance. Since our state is Gaussian, this completely describes the state.

• is the mean of x at k given at k − 1

• is the mean of y at k given at k − 1

means real numbers in a vector of order n, that is, the state has n quantities. In probabilistic terms the model is:

(4.55)

(4.56)

The integrals become simple matrix equations. In the following equations, means the covariance prior to the measurement update.

(4.57)

(4.58)

(4.59)

(4.60)

(4.61)

The prediction step becomes:

(4.62)

(4.63)

The first term in the above covariance equation propagates the covariance based on the state transition matrix, A. Q_k+1 adds to this to form the next covariance. Process noise Q_k+1 is a measure of the accuracy of the mathematical model, A, in representing the system. For example, suppose A was a mathematical model that damped all states to zero. Without Q, P would go to zero. But if we really weren’t that certain about the model, the covariance would never be less than Q. Picking Q can be difficult. In a dynamical system with uncertain disturbances you can compute the standard deviation of the disturbances to compute Q. If the model, A were uncertain, then you might do a statistical analysis of the range of models. Or you can try different Q in simulation and see which ones work the best!

The update step is:

(4.64)

(4.65)

(4.66)

(4.67)

(4.68)

S_k is an intermediate quantity. v_k is the residual. The residual is the difference between the measurement and your estimate of the measurement given the estimated states. R is just the covariance matrix of the measurements. If the noise is not white, a different filter should be used. White noise has equal energy at all frequencies. Many types of noise, such as the noise from an imager, is not really white noise, but is band limited, that is, it has noise in a limited range of frequencies. You can sometimes add additional states to A to model the noise better, for example, adding a low-pass filter to band limit the noise. This makes A bigger, but is generally not an issue.

Kalman Filter Implementation

Now we will implement a Kalman Filter estimator for the mass-spring oscillator. First, we need a method of converting the continuous time problem to discrete time. We only need to know the states at discrete times or at fixed intervals, T. We use the continuous to discrete transform, which uses the MATLAB expm function, which performs the matrix exponential. This transform is coded in CToDZOH, the body of which is shown in the following listing. T is the sampling period.

 [n,m] =  size (b);

 q     =  expm ([a*T b*T; zeros (m,n+m)]);

 f     = q(1:n,1:n);

 g     = q(1:n,n+1:n+m);  

CToDZOH includes a demo for a double integrator. A double integrator is a system in which the second derivative of the state is directly dependent upon an external input. In this example, x is the state, representing a position, and a is an external input of acceleration.

(4.69)

Written in state space form it is:

(4.70)

(4.71)

or in matrix form

(4.72)

where

(4.73)

(4.74)

(4.75)

(4.76)

To run the demo, simply run CToDZOH from the command line without any inputs.

 >> CToDZOH

 Double integrator with a  0.5 second time step.

 a =

      0     1

      0     0

 b =

      0

      1

 f =

     1.0000    0.5000

          0    1.0000

 g =

     0.1250

     0.5000  

The discrete plant matrix f is easy to understand. The position state at step k + 1 is the state at k plus the velocity at step k multiplied by the time step T of 0.5 s. The velocity at step k + 1 is the velocity at k plus the time step times the acceleration at step k. The acceleration at the time k multiplies to get the contribution to the position. This is just the standard solution to a particle under constant acceleration.

(4.77)

(4.78)

In matrix form this is:

(4.79)

With the discrete time approximation, we can change the acceleration every step k to get the time history. This assumes that the acceleration is constant over the period T. We need to pick T to be sufficiently small so that this is approximately true if we are to get good results.

The script for testing the Kalman Filter is KFSim.m. KFInitialize is used to initialize the filter (a Kalman Filter, ’kf’, in this case). This function has been written to handle multiple types of Kalman Filters and we will use it again in the recipes for EKF and UKF (’ekf’ and ’ukf’ respectively). We show it below. This function uses dynamic field names to assign the input values to each field.

The simulation starts by assigning values to all of the variables used in the simulation. We get the data structure from the function RHSOscillator and then modify its values. We write the continuous time model in matrix form and then convert it to discrete time. Note that the measurement equation matrix that multiples the state, h, is [1 0], indicating that we are measuring the position of the mass. MATLAB’s randn random number function is used to add Gaussian noise to the simulation. The rest of the script is the simulation loop with plotting afterward.

The first part of the script creates continuous time state space matrices and converts them to discrete time using CToDZOH. You then use KFInitialize to initialize the Kalman Filter.

  %% Initialize

 tEnd          = 100.0;             % Simulation end time (sec)

 dT            = 0.1;               % Time step (sec)

 d             = RHSOscillator();   % Get the default data structure

 d.a           = 0.1;               % Disturbance acceleration

 d.omega       = 0.2;               % Oscillator frequency

 d.zeta        = 0.1;               % Damping ratio

 x             = [0;0];             % Initial state [position;velocity]

 y1Sigma       = 1;                 % 1 sigma position measurement noise

  % xdot = a*x + b*u

 a = [0 1;-2*d.zeta*d.omega -d.omega^2];  % Continuous time model

 b = [0;1];                               % Continuous time input matrix

  % x[k+1] = f*x[k] + g*u[k]

 [f,g] = CToDZOH(a,b,dT);   % Discrete time model

 xE    = [0.3; 0.1];        % Estimated initial state

 q     = [1e-6 1e-6];       % Model noise covariance ;

                            % [1e-6 1e-6] is for low model noise test

                            % [1e-4 1e-4] is for high model noise test

 dKF   = KFInitialize( ’kf’, ’m’,xE, ’a’,f, ’b’,g, ’h’,[1 0],...

                       ’r’,y1Sigma^2, ’q’, diag (q), ’p’, diag (xE.^2));  

The simulation loop cycles through measurements of the state and the Kalman Filter update and prediction state with the code KFPredict and KFUpdate. The integrator is between the two to get the phasing of the update and prediction correct. You have to be careful to put the predict and update steps in the right places in the script so that the estimator is synchronized with simulation time.

  %% Simulation

 nSim  =  floor (tEnd/dT) + 1;

 xPlot =  zeros (5,nSim);

 for k = 1:nSim

    % Position measurement with random noise

   y = x(1) + y1Sigma* randn (1,1);

    % Update the Kalman Filter

   dKF.y = y;

   dKF   = KFUpdate(dKF);

    % Plot storage

   xPlot(:,k) = [x;y;dKF.m-x];

    % Propagate (numerically integrate) the state equations

   x = RungeKutta( @RHSOscillator, 0, x, dT, d );

    % Propagate the Kalman Filter

   dKF.u = d.a;

   dKF   = KFPredict(dKF);

 end  

The prediction Kalman Filter step, KFPredict, is shown in the following listing with an abbreviated header. The prediction propagates the state one time step and propagates the covariance matrix with it. It is saying that when we propagate the state, there is uncertainty, so we must add that to the covariance matrix.

  %% KFPREDICT Linear Kalman Filter prediction step.

 function d = KFPredict( d )

  % The first path is if there is no input matrix b

 if( isempty(d.b) )

   d.m = d.a*d.m;

 else

   d.m = d.a*d.m + d.b*d.u;

 end

 d.p = d.a*d.p*d.a’ + d.q;

The update Kalman Filter step, KFUpdate, is shown in the following listing. This adds the measurements to the estimate and accounts for the uncertainty (noise) in the measurements.

  %% KFUPDATE Linear Kalman Filter measurement update step.

 function d = KFUpdate( d )

 s   = d.h*d.p*d.h’ + d.r;        % Intermediate value

 k   = d.p*d.h’/s;          % Kalman gain

 v   = d.y - d.h*d.m;       % Residual

 d.m = d.m + k*v;           % Mean update

 d.p = d.p - k*s*k’;        % Covariance update

You will note that the “memory” of the filter is stored in the data structure d. No persistent data storage is used. This makes it easier to use these functions in multiple places in your code. Note also that you don’t have to call KFUpdate every time step. You need only call it when you have new data. However, the filter does assume uniform time steps.

The script gives two examples for the model noise covariance matrix. Figure 4.7 shows results when high numbers, [1e-4 1e-4], for the model covariance are used. Figure 4.8 shows results when lower numbers, [1e-6 1e-6], are used. We don’t change the measurement covariance because only the ratio between noise covariance and model covariance is important.

When the higher numbers are used, the errors are Gaussian but noisy. When the low numbers are used, the result is very smooth, with little noise seen. However, the errors are large in the low model covariance case. This is because the filter is essentially ignoring the measurements, since it thinks the model is very accurate. You should try different options in the script and see how it performs. As you can see, the parameters make a huge difference in how well the filter learns about the states of the system.

4.2.1 Problem

We want to track the damped oscillator using an EKF with the nonlinear angle measurement. The EKF was developed to handle models with nonlinear dynamical models and/or nonlinear measurement models. The conventional, or linear, filter requires linear dynamical equations and linear measurements models, that is, the measurement is a linear function of the state. If the model is not linear, linear filters will not track the states very well.

Given a nonlinear model of the form:

(4.80)

(4.81)

The prediction step is:

(4.82)

(4.83)

F is the Jacobian of f. The update step is:

(4.84)

(4.85)

(4.86)

(4.87)

(4.88)

F_x(m, k − 1) and H_x(m, k) are the Jacobians of the nonlinear functions f and h. The Jacobians are just a matrix of partial derivatives of F and H. This results in matrices from the vectors F and H. For example, assume we have f(x, y), which is:

(4.89)

The Jacobian is

(4.90)

The matrix is computed at x_k, y_k.

The Jacobians can be found analytically or numerically. If done numerically, the Jacobian needs to be computed about the current value of m_k. In the Iterated EKF, the update step is done in a loop using updated values of m_k after the first iteration. H_x(m, k) needs to be updated at each step.

4.2.2 Solution

We will use the same KFInitialize function as created in the previous recipe, but now using the ’ekf’ input. We will need functions for the derivative of the model dynamics, the measurement, and the measurement derivatives. These are implemented in RHSOscillator Partial, AngleMeasurement, and AngleMeasurementPartial.

We will also need custom versions of the filter to predict and update steps.

4.2.3 How It Works

The EKF requires a measurement function, a measurement derivative function, and a state derivative function. The state derivative function computes the a matrix:

(4.91)

You would only use the EKF if a_k changed with time. In this problem, it does not. The function to compute a is RHSOscillatorPartial. It uses CToDZOH. We could have computed a once, but using CToDZOH makes the function more general.

Our measurement is nonlinear (being an arctangent) and needs to be linearized about each value of position. AngleMeasurement computes the measurement, which is nonlinear but smooth.

AngleMeasurementPartial computes the derivative. The following function computes the c matrix

(4.92)

The partial measurement if found by taking the derivative of the arc-tangent of the angle from the baseline. The comment reminds you of this fact.

It is convenient that the measurement function is smooth. If there were discontinuities, the measurement partials would be difficult to compute. The EKF implementation can handle either functions for the derivatives or matrices. In the case of the functions, we use feval to call them. This can be seen in the EKFPredict and EKFUpdate functions.

EKFPredict is the state propagation step for an EKF. It numerically integrates the right-hand side using RungeKutta. RungeKutta may be overkill in some problems and a simple Euler integration may be appropriate. Euler integration is just:

(4.93)

where f(x, u, t) is the right-hand side that can be a function of the state, x, time t, and the inputs u.

The EKFSim script implements the EKF with all of the above functions as shown in the following listing. The functions are passed to the EKF in the data structure produced by KFInitialize. Note the use of function handles using @, i.e., @RHSOscillator. Notice that KFInitialize requires hX and fX for computing partial derivatives of the dynamical equations and measurement equations.

Figure 4.9 shows the results. The errors are small. Since the problem dynamics are linear, we don’t expect any differences from a conventional Kalman Filter.

4.3.1 Problem

You want to learn the states of the spring, damper, mass system given a nonlinear angle measurement. This time we’ll use an UKF. With the UKF, we work with the nonlinear dynamical and measurement equations directly. We don’t have to linearize them as we did for the EKF with RHSOscillatorPartial and AngleMeasurementPartial. The UKF is also known as a sigma σ point filter because it simultaneously maintains models one sigma (standard deviation) from the mean.

4.3.2 Solution

We will create an UKF as a state estimator. This will absorb measurements and determine the state. It will autonomously learn about the state of the system based on a pre-existing model.

In the following text we develop the equations for the non-augmented Kalman Filter. This form only allows for additive Gaussian noise. Given a nonlinear model of the form

(4.94)

(4.95)

Define weights as

(4.96)

(4.97)

(4.98)

(4.99)

m are weights on the mean state( m for mean) and c weights on the covariances. Note that .

(4.100)

(4.101)

c scales the covariances to compute the sigma points, that is, the distribution of points around the mean for computing the additional states to propagate. α, β, and κ are scaling constants. General rules for the scaling constants are:

• α – 0 for state estimation, 3 minus the number of states for parameter estimation.

• β – Determines the spread of sigma points. Smaller means more closely spaced sigma points.

• κ – Constant for prior knowledge. Set to 2 for Gaussian processes.

n is the order of the system. The weights can be put into matrix form:

(4.102)

(4.103)

I is the 2n + 1 by 2n + 1 identity matrix. In the equation vector w_m is replicated 2n + 1 times. W is 2n + 1 by 2n + 1.

The prediction step is:

(4.104)

(4.105)

(4.106)

(4.107)

where X is a matrix where its column is the state vector possibly with an added sigma point vector. The update step is:

(4.108)

(4.109)

(4.110)

(4.111)

(4.112)

(4.113)

(4.114)

(4.115)

μ_k is a matrix of the measurements in which each column is a copy modified by the sigma points. S_k and C_k are intermediate quantities. The brackets around are just for clarity.

4.3.3 How It Works

The weights are computed in UKFWeight.

The prediction UKF step is shown in the following excerpt from UKFPredict.

UKFPredict uses RungeKutta for prediction, which is done by numerical integration. In effect, we are running a simulation of the model and just correcting the results with the next function, UKFUpdate. This gets to the core of the Kalman Filter. It is just a simulation of your model with a measurement correction step. In the case of the conventional linear Kalman Filter, we use a linear discrete time model.

The update UKF step is shown in the following listing. The update propagates the state one time step.

The sigma points are generated using chol. chol is Cholesky factorization and generates an approximate square root of a matrix. A true matrix square root is more computationally expensive and the results don’t really justify the penalty. The idea is to distribute the sigma points around the mean and chol works well. Here is an example that compares the two approaches:

The square root actually produces a square root! The diagonal of b*b is close to z, which is all that is important.

The script for testing the UKF, UKFSim, is shown below. As noted earlier, we don’t need to convert the continuous time model into discrete time as we did for the Kalman Filter and EKF. Instead, we pass the filter the right-hand side of the differential equations. You must also pass it a measurement model, which can be nonlinear. You add UKFUpdate and UKFPredict function calls to the simulation loop. We start by initializing all parameters. KFInitialize takes parameter pairs, after ’ukf’ to initialize the filter. The remainder is the simulation loop and plotting. Initialization requires computation of the weighting matrices after calling KFInitialize.

We show the simulation loop here:

The results are shown in Figure 4.10. The errors Δr_E and Δv_E are just noise. The measurement goes over a large angle range which would make a linear approximation problematic.

4.4.1 Problem

You want to learn the parameters of the spring-damper-mass system given a nonlinear angle measurement. The UKF can be configured to do this.

4.4.2 Solution

The solution is to create a UKF configured as a parameter estimator. This will absorb measurements and determine the undamped natural frequency. It will autonomously learn about the system based on a pre-existing model. We develop the version that requires an estimate of the state that could be generated with a UKF running in parallel, as in the previous recipe.

4.4.3 How It Works

Initialize the parameter filter with the expected value of the parameters, η [28]

(4.116)

and the covariance for the parameters

(4.117)

The update sequence begins by adding the parameter model uncertainty, Q, to the covariance, P,

(4.118)

Q is for the parameters, not the states. The sigma points are then calculated. These are points found by adding the square root of the covariance matrix to the current estimate of the parameters.

(4.119)

γ is a factor that determines the spread of the sigma points. We use chol for the square root. If there are L parameters, the P matrix is L × L, so this array will be L × (2L + 1).

The state equations are of the form:

(4.120)

and the measurement equations are:

(4.121)

x is the previous state of the system, as identified by the state estimator or other process. u is a structure with all other inputs to the system that are not being estimated. η is a vector of parameters that are being estimated and t is time. y is the vector of measurements. This is the dual estimation approach in that we are not estimating x and η simultaneously.

The script UKFPSim for testing the UKF parameter estimation is shown below. We are not doing the UKF state estimation to simplify the script. Normally, you would run the UKF in parallel. We start by initializing all parameters. KFInitialize takes parameter pairs to initialize the filters. The remainder is the simulation loop and plotting. Notice that there is only an update call, since parameters, unlike states, do not propagate.

The UKF parameter update function is shown in the following code. It uses the state estimate generated by the UKF. As noted, we are using the exact value of the state generated by the simulation. This function needs a specialized right-hand side that uses the parameter estimate, d.eta. We modified RHSOscillator for this purpose and wrote RHSOscillatorUKF.

It also has its own weight initialization function UKFPWeight.m. The weight matrix is used by the matrix form of the Unscented Transform. The constant alpha determines the spread of the sigma points around the parameter vector and is usually set to between 10e-4 and 1. beta incorporates prior knowledge of the distribution of the parameter vector and is 2 for a Gaussian distribution. kappa is set to 0 for state estimation and 3 the number of states for parameter estimation.

RHSOscillatorUKF is the oscillator model used by the UKF. It has a different input format than RHSOscillator. There is only one line of code.

LinearMeasurement is a simple measurement function for demonstration purposes. The UKF can use arbitrarily complex measurement functions.

The results of a simulation of an undamped oscillator are shown in Figure 4.11. The filter rapidly estimates the undamped natural frequency. The result is noisy, however. You can explore this script by varying the numbers in the script.