%% Estimate States Online Using Unscented Kalman Filter
% Estimate the states of a van der Pol oscillator using an unscented Kalman filter algorithm and measured output data. The oscillator has two states and one output.
% Create an unscented 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 nonlinearity parameter, 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, using knowledge of system outputs until time k-1, \hat x[k|k-1].

obj = unscentedKalmanFilter(@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 unscented Kalman filter algorithm to estimate the states of the oscillator by using the correct and predict commands. You first correct \hat x[k|k-1] using measurements at time k to get \hat x[k|k]. Then, you predict the state value at next time step, \hat x[k+1|k], using  \hat 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:size(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.
% In this example, you used correct before predict because the initial state value was  \hat x[k|k-1], a guess for the state value at initial time k using system outputs until time k-1. If your initial state value is  \hat x[k-1|k-1], the value at previous time k-1 using measurement 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 postcorrection 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.
% Copyright 2012-2019 The MathWorks, Inc.