DoubleWell

% 
% dX = -(a*X + b*X^3) dt + sigma dW
%
% Corresponds to particle in potential U = 1/2 a x^2 + 1/4 b x^4.
% If a < 0 and b > 0, the potential has a double well.
%
% We use measurements with noise given by N(0,R)

function DoubleWell
    
a = -1;
b = 0.1;
sigma = 2;  % process noise 
R = 1;  % measurement noise covariance

f = @(x,t) (-a*x - b*x.^3);
J_f = @(x,t) (-a - 3*b*x.^2);

endTime = 100;
dt = 0.1;
iter = endTime/dt;

trueState = 0;

% initial state
initMean = 0;
initCov = 10000;

ukfSysModel = UnscentedContinuousSystemModel(f, sigma);
ekfSysModel = EKFContinuousSystemModel(f, sigma, J_f);
obsModel = LinearObserverModel(1, R);  % direct observation of noisy measurements

ukfFilt = BLUEFilter(ukfSysModel, obsModel);
ekfFilt = BLUEFilter(ekfSysModel, obsModel);

stateVec = zeros([1 iter]); % true state
stateVec(:,1) = trueState;

measVec = zeros([1 iter]); % measurements

for i = 2:iter
    % simulate time evolution and measurement
    trueState = ukfSysModel.simulate(trueState, dt);
    observation = obsModel.simulate(trueState);

    stateVec(:, i) = trueState;
    measVec(i) = observation;
end

% function to perform the filter
function predVec = doFilter(filt)
     predVec = zeros(1, iter);
     predVec(1) = initMean;
     
     predCov = zeros(1,1,iter);
     predCov(1,1,1) = initCov;
     
     for i = 2:iter
         %predDist(:,i) = filt.filter(predDist(:,i-1), measVec(i), dt);
         [predVec(i), predCov(1,1,i)] = filt.filter(predVec(i-1), ...
                                       predCov(1,1,i-1), measVec(i), dt);
     end
end

ekfPrediction = doFilter(ekfFilt);
ukfPrediction = doFilter(ukfFilt);

times = dt * (0:iter-1);

figure
hold on;
plot(times, stateVec(1,:), 'b-');
plot(times, ekfPrediction(1,:), 'r-');
plot(times, ukfPrediction(1,:), 'g-');
legend('Actual state', 'EKF prediction', 'UKF prediction');
hold off;

figure
hold on;
plot(times, ekfPrediction(1,:) - stateVec(1,:), 'r-');
plot(times, ukfPrediction(1,:) - stateVec(1,:), 'g-');
legend('EKF error', 'UKF error');
hold off;

end