clear all        % to be sure what you are doing...
format compact   % for better readability.

% First, we define an ACTUAL plant and a MODEL of the plant.
% These SHOULD be the SAME, in general, but we also want to 
% examine how sensitive our results are if they are DIFFERENT.
% In variable names:
%    "c" stands for "continuous" time. "d" for "discrete".
%    "p" stands for "plant", and "m" stands for "model".

% === Define the PLANT and MODEL dynamics:
wn_p = 5; % actual (p="plant") natural frequency of pendulum, rad/s
zeta_p = .2;  % damping ratio--> d2x + 2*zeta*wn*dx + wn^2*x
wn_m =    % modeled (m="model") natural frequency
zeta_m =   
% ------ PLANT:
Acp = [0 1; -wn_p^2 -2*zeta_p*wn_p];  % CONTINUOUS TIME PLANT dynamics
Bcp = [0; 1];
Ccp = [1 0];
Dcp = [0];
sscp = ss(Acp,Bcp,Ccp,Dcp);  % CONTINUOUS TIME state-space PLANT system
% ------ MODEL:
Acm = [0 1; -wn_m^2 -2*zeta_m*wn_m];  % CONTINUOUS TIME MODEL dynamics
Bcm = [0; 1];
Ccm = [1 0];
Dcm = [0];
sscm = ss(Acm,Bcm,Ccm,Dcm);  % CONTINUOUS TIME state-space MODEL system

% Now, simulate PLANT and MODEL as a discrete-time systems:
T = 1e-3;  % Sample time (seconds)
ssdm = c2d(sscm,T);
ssdp = c2d(sscp,T);
Adm = ssdm.A; Bdm = ssdm.B; Cdm = ssdm.C; Ddm = ssdm.D; % MODEL
Adp = ssdp.A; Bdp = ssdp.B; Cdp = ssdp.C; Ddp = ssdp.D; % PLANT

% Create an estimator given a "no noise" assumption:
pc = eig(Acm) % poles of plant


pe =               % Set poles of estimator...
% Below, calculate observer gains (NOT Kalman filter; no noise assumed)
Ld =                        % can use "place" or "acker"

% Initialize state and other variables for simulation loop:
x0 = [20*pi/180; 0];  % angle and velocity
y0 = 0;               % start with false estimate of state
ndo = 1e4;         % Number of time steps in simulation
t = [0:ndo-1]*T;   % Corresponding time vector
x = 0*x0*t;        % pre-allocation (for MATLAB) of entire vector "x"
x(:,1) = x0;  % PLUG IN the INITIAL CONDITIONS (position and velocity)
y = 0*t;           % actual measurement (pre-allocation)
xe = x;            % estimated state (pre-allocation)
ye = y;            % estimated measurement (pre-allocation)
u = 0*t;  % no torque; you can modify, if desired...
P = zeros(2,2); % initialize covariance matrix
Psave = zeros(4,ndo);  % save matrix coefficients over time....
H = Cdm;

% Below, try playing with v and w, vs Q and R...
w = .001;  % noise on process (forces and torques)
v = 2*pi/(2^12); % noise on MEASUREMENT...
Q =      % try changing, so not equal to actual "w"
R =              % try changing, so not equal to actual "v"
for n=2:ndo
    % ACTUAL PLANT DYNAMICS:  Use Adp, Bdp, Cdp and Ddp here.
    x(:,n) = Adp*x(:,n-1) + Bdp*u(n-1) + [0; randn*w];
    y(:,n) = Cdp*x(:,n) + Ddp*u(n) + randn*v;
    
    % INTERNAL MODEL OF PLANT: Use Adm, Bdm, Cdm and Ddm below.
    
    % --- Predictor / Time Update steps:
    % (1) Predictor: a priori estimated state (time update)
    xe(:,n) = 
    % (2) Predictor: a priori covariance (time update) 
    P =  
    
    % --- Corrector / Measurement Update steps:
    % (3) Corrector: Kalman gain (or, replace with a constant estimator!)
    
    Lkalman =
    
    %%%%%%%%%%%%% BELOW IS A "TRICK", TO COMPARE ESTIMATORS: %%%%%%
    % CHEAT below: substitute the ESTIMATOR of your choice...
    Luse = Ld;
    %Luse = Lkalman;
    
    % (4) Corrector: a posteriori estimated state (measurement update)
    xe(:,n) = 
    % (5) Corrector: a posteriori estimated covariance (meas. update)
    P = 
    
    % I also saved ye and Psave, just to look at them later...
    ye(:,n) = Cdm*xe(:,n);
    Psave(:,n) = P(:);  % keep a running log of P, over time...
end

% Then, plot results:
figure(1); clf
subplot(211)
plot(t,x(1,:)); hold on; grid on
plot(t,xe(1,:),'r--'); xlabel('Time (s)')
ylabel('Angle (rad)')
legend('Actual state','Estimated state')
subplot(212)
plot(t,x(2,:)); hold on; grid on
plot(t,xe(2,:),'r--'); xlabel('Time (s)')
ylabel('Angular velocity (rad/s)')
legend('Actual state','Estimated state')
    

