%% Extended Kalman filter
%
%
%%
%
%% Specify parameters

deltaT = 2e-6;
t_max = 1e-3; 
time = 0:deltaT:t_max;
R = 10;
C1 = 1e-6;
C2 = 1e-6;
sigma_V = 0.2; % standard deviation of voltage measurements
frequency = 5000; % power supply frequency (Hz)
Vi_function = @(t)10*sin(2*pi*frequency*t); % provide as a function for the benefit of the ODE solver below
Vi = Vi_function(time);

%
%% Calculate an accurate solution using ODE solvers
% We can use the ordinary differential equation (ODE) solvers in Matlab to produce an accurate solution to this set of equations.
%

x0 = [0; 0]; % initial conditions
% this function computes dx/dt for the state vector x
odefun = @(t,x)[
    (-x(1)-x(2))/(R*C1) + Vi_function(t)/(R*C1);
    (-x(1)-x(2))/(R*C2) - 10^-12/C2*(exp(x(2)/0.026) - 1) + Vi_function(t)/(R*C2);
];
% the solver returns the state vector at each requested time step
[~, exact] = ode23(odefun, time, x0);
exact_V1 = exact(:,1);
exact_V2 = exact(:,2);
figure();
plot(time, exact_V1);
xlabel('Time (s)');
ylabel('V_1 (volts)');
title('Voltage across C_1');

figure();
plot(time, exact_V2);
xlabel('Time (s)');
ylabel('V_2 (volts)');
title('Voltage across C_2');

%
% Notice how V2 is clamped to approximately 0.6V in the positive direction because that is when the diode just begins to conduct. Hence the circuit behaves like an AC voltage divider but with a DC offset controlled by the diode.
%
%% Simulate noisy measurements

measurement_covariance = [sigma_V^2     0
                          0             sigma_V^2];
measurements = mvnrnd(exact, measurement_covariance);

figure();
plot(time, exact_V1);
hold on;
plot(time, measurements(:,1), '.');
xlabel('Time (s)');
ylabel('V_1 (volts)');
title('Noisy measurements')

%%
%
%% Kalman filter
%

% Implement your code here.

% You will need to set up the initial state vector and covariance, then
% loop through all time steps and run the UFK algorithm.

x = x0; % initial state vector

% Here is a suggestion for the process covariance:
Q = diag([1 1] * (0.1 * frequency * deltaT)^2); % accumulate 0.1 V of standard deviation per cycle 


% Allocate a variable for the the output of the filter at each time step
kalman_V1 = zeros(size(exact_V1));
kalman_V2 = zeros(size(exact_V2));


% Write your code here


for k = 2:length(time)
    % Write the UKF algorithm here.
    
    
    % Here's how to use the prediction model:
    % Suppose X is one of the columns in your matrix of sigma points.
    % The code below will run this column through the prediction model.
    %
    % F(:, i) = predict(X, time(k), deltaT, R, C1, C2, Vi_function);
    %
    % The function 'predict' is defined at the bottom of this file.
    
    % Write your updated state vector back into the variable 'x' so it can
    % be saved by the code below for plotting. 
    
    % Save values 
    kalman_V1(k) = x(1);
    kalman_V2(k) = x(2);
end

figure();
plot(time, exact_V1);
hold on;
plot(time, kalman_V1);
xlabel('Time (s)');
ylabel('V_1 (volts)');
legend({'Exact', 'UKF'});

figure();
plot(time, exact_V2);
hold on;
plot(time, kalman_V2);
xlabel('Time (s)');
ylabel('V_2 (volts)');
legend({'Exact', 'UKF'});

%



%

function x_pred = predict(x, time, deltaT, R, C1, C2, Vi_function)
    % Prediction model for the circuit.
    % 
    % Uses a proper ODE solver to integrate forward in time.
    % The input parameters are:
    %  x: state vector
    %  time: current time (seconds)
    %  deltaT: amount of time to integrate forward
    %  R, C1, C2: circuit element resistance and capacitance
    %  Vi_function: function handle giving the input voltage source, e.g. a
    %  sinusoid or some other time varying function.
    % 
    odefun = @(t,x)[
        (-x(1)-x(2))/(R*C1) + Vi_function(t)/(R*C1);
        (-x(1)-x(2))/(R*C2) - 10^-12/C2*(exp(x(2)/0.026) - 1) + Vi_function(t)/(R*C2);
    ];

    % the solver returns the state vector at each requested time step
    [~, solution] = ode23(odefun, time+[0 deltaT], x, odeset('AbsTol', 1e-8, 'RelTol', 1e-8, 'InitialStep',deltaT/1000, 'MaxStep', deltaT/10));
    x_pred = solution(end,:)';
end