correct

Estimate the states of a van der Pol oscillator using an extended Kalman filter algorithm and measured output data. The oscillator has two states and one output.

Create an extended Kalman filter object for the oscillator. Use previously written and saved state transition and measurement functions, vdpStateFcn.m and vdpMeasurementFcn.m. These functions describe a discrete-approximation to a van der Pol oscillator with the nonlinearity parameter $\mathrm{mu}$ equal to 1. The functions assume additive process and measurement noise in the system. Specify the initial state values for the two states as [1;0]. This is the guess for the state value at initial time k, based on knowledge of system outputs until time k-1, $$\underset{}{\overset{ˆ}{x}}[k|k-1]$$.

obj = extendedKalmanFilter(@vdpStateFcn,@vdpMeasurementFcn,[1;0]);

Load the measured output data y from the oscillator. In this example, use simulated static data for illustration. The data is stored in the vdp_data.mat file.

load vdp_data.mat y

Specify the process noise and measurement noise covariances of the oscillator.

obj.ProcessNoise = 0.01;
obj.MeasurementNoise = 0.16;

Initialize arrays to capture results of the estimation.

residBuf = [];
xcorBuf = [];
xpredBuf = [];

Implement the extended Kalman filter algorithm to estimate the states of the oscillator by using the correct and predict commands. You first correct $$\underset{}{\overset{ˆ}{x}}[k|k-1]$$ using measurements at time k to get $$\underset{}{\overset{ˆ}{x}}[k|k]$$. Then, you predict the state value at the next time step $$\underset{}{\overset{ˆ}{x}}[k+1|k]$$ using $$\underset{}{\overset{ˆ}{x}}[k|k]$$, the state estimate at time step k that is estimated using measurements until time k.

To simulate real-time data measurements, use the measured data one time step at a time. Compute the residual between the predicted and actual measurement to assess how well the filter is performing and converging. Computing the residual is an optional step. When you use residual, place the command immediately before the correct command. If the prediction matches the measurement, the residual is zero.

After you perform the real-time commands for the time step, buffer the results so that you can plot them after the run is complete.

for k = 1:length(y)
    [Residual,ResidualCovariance] = residual(obj,y(k));
    [CorrectedState,CorrectedStateCovariance] = correct(obj,y(k));
    [PredictedState,PredictedStateCovariance] = predict(obj);
    
     residBuf(k,:) = Residual;
     xcorBuf(k,:) = CorrectedState';
     xpredBuf(k,:) = PredictedState';
end

When you use the correct command, obj.State and obj.StateCovariance are updated with the corrected state and state estimation error covariance values for time step k, CorrectedState and CorrectedStateCovariance. When you use the predict command, obj.State and obj.StateCovariance are updated with the predicted values for time step k+1, PredictedState and PredictedStateCovariance. When you use the residual command, you do not modify any obj properties.

In this example, you used correct before predict because the initial state value was $$\underset{}{\overset{ˆ}{x}}[k|k-1]$$, a guess for the state value at initial time k based on system outputs until time k-1. If your initial state value is $$\underset{}{\overset{ˆ}{x}}[k-1|k-1]$$, the value at previous time k-1 based on measurements until k-1, then use the predict command first. For more information about the order of using predict and correct, see Using predict and correct Commands.

Plot the estimated states, using the post-correction values.

plot(xcorBuf(:,1),xcorBuf(:,2))
title("Estimated States")

Plot the actual measurement, the corrected estimated measurement, and the residual. For the measurement function in vdpMeasurementFcn, the measurement is the first state.

M = [y,xcorBuf(:,1),residBuf];
plot(M)
grid on
title("Actual and Estimated Measurements, Residual")
legend("Measured","Estimated","Residual")

The estimate tracks the measurement closely. After the initial transient, the residual remains relatively small throughout the run.

Load the van der Pol ODE data, and specify the sample time.

vdpODEdata.mat contains a simulation of the van der Pol ODE with nonlinearity parameter mu=1, using ode45, with initial conditions [2;0]. The true state was extracted with sample time dt = 0.05.

load ('vdpODEdata.mat','xTrue','dt')
tSpan = 0:dt:5;

Get the measurements. For this example, a sensor measures the first state with a Gaussian noise with standard deviation 0.04.

sqrtR = 0.04;
yMeas = xTrue(:,1) + sqrtR*randn(numel(tSpan),1);

Create a particle filter, and set the state transition and measurement likelihood functions.

myPF = particleFilter(@vdpParticleFilterStateFcn,@vdpMeasurementLikelihoodFcn);

Initialize the particle filter at state [2; 0] with unit covariance, and use 1000 particles.

initialize(myPF,1000,[2;0],eye(2));

Pick the mean state estimation and systematic resampling methods.

myPF.StateEstimationMethod = 'mean';
myPF.ResamplingMethod = 'systematic';

Estimate the states using the correct and predict commands, and store the estimated states.

xEst = zeros(size(xTrue));
for k=1:size(xTrue,1)
    xEst(k,:) = correct(myPF,yMeas(k));
    predict(myPF);
end

Plot the results, and compare the estimated and true states.

figure(1)
plot(xTrue(:,1),xTrue(:,2),'x',xEst(:,1),xEst(:,2),'ro')
legend('True','Estimated')

Consider a nonlinear system with input u whose state x and measurement y evolve according to the following state transition and measurement equations:

The process noise w of the system is additive while the measurement noise v is nonadditive.

Create the state transition function and measurement function for the system. Specify the functions with an additional input u.

f = @(x,u)(sqrt(x+u));
h = @(x,v,u)(x+2*u+v^2);

f and h are function handles to the anonymous functions that store the state transition and measurement functions, respectively. In the measurement function, because the measurement noise is nonadditive, v is also specified as an input. Note that v is specified as an input before the additional input u.

Create an extended Kalman filter object for estimating the state of the nonlinear system using the specified functions. Specify the initial value of the state as 1 and the measurement noise as nonadditive.

obj = extendedKalmanFilter(f,h,1,'HasAdditiveMeasurementNoise',false);

Specify the measurement noise covariance.

obj.MeasurementNoise = 0.01;

You can now estimate the state of the system using the predict and correct commands. You pass the values of u to predict and correct, which in turn pass them to the state transition and measurement functions, respectively.

Correct the state estimate with measurement y[k]=0.8 and input u[k]=0.2 at time step k.

correct(obj,0.8,0.2)

Predict the state at the next time step, given u[k]=0.2.

predict(obj,0.2)

Retrieve the error, or residual, between the prediction and the measurement.

[Residual, ResidualCovariance] = residual(obj,0.8,0.2);