%% Finite-time averaging idealized measurement process; continuous-time vs discrete-time Kalman filter: Matlab example
% © 2025, Antonio Sala Piqueras, Universitat Politecnica de Valencia, Spain. All rights reserved.
% Presentacions in video:
% https://personales.upv.es/asala/YT/V/medabsEN.html  [generic discussion, abstract interpretation, no Matlab code]
% https://personales.upv.es/asala/YT/V/medex1EN.html [preliminaries, discretization and simulation of example stochastic process]
% https://personales.upv.es/asala/YT/V/medex2EN.html [continuous-time and discrete-time Kalman filters, comparison, finite difference estimation variance]
%
% Objective: understanding the "low-level sensing" behaviour, comparing a continuous-time Kalman Filter with a discrete-time one assuming measurement comes as y_{meas}=\frac{1}{T_s}\int_0^{T_s}(y(\tau)+measurementnoise)\,d\tau with some PSD for the measurement noise, via a numeric example.
%
% Table of Contents
% Base process to measure
%         Measurement device (idealized)
%         Simulation  (realization of stochastic process)
% Position estimation via averaging (finite-time integral)
%     Comparison with discrete-time Kalman filter
%     Converging to the continuous-time Kalman filter
% Optimal sampling period (for non-Kalman approach)
%%
%% Base process to measure
% We will have a 2nd order continuous-time process.

wn=0.25; %natural frequency
dampingcoeff=.7071;
Areduced=[0 1;-wn^2 -2*dampingcoeff*wn];
Greduced=[0;1];
eig(Areduced)
Cr=[1 0];
sys_reduced=ss(Areduced,Greduced,Cr,0);
zpk(sys_reduced)

%% Measurement device (idealized)
% It will connect to a device, say an RC so that the capacitor is charged during some conversion time, etc. Say, the measurement will be \frac{1}{T_s}\int_0^{T_s} y(\tau)d\tau... but it will add some noise... It is the "optimal filter" for constant y and zero prior information (infinite covariance); if y is not constant, the measurement will "blur" it.
%  In theory, we reset the integrator after each measurement but, well, measuring its value with "zero error" will be the same, as increments will be trivially computed:
% \frac{1}{T_s}\int_a^{a+T_s} y(\tau)d\tau=\frac{1}{T_s}\cdot \left(\int_0^{a+T_s} y(\tau)d\tau-\int_0^{a} y(\tau)d\tau\right) \\
% =\frac{1}{T_s}\cdot (q(a+T_s)-q(a))
% being q(t) the state of the integrator and a any arbitrary time instant when data acquisition begins.
%
% And, well, we may have some pole -\epsilon instead of pure integrator... This will help us in getting finite steady-state variance and 1/(s+\epsilon) is basically an integrator unless sampling frequency is extremely small.

epsi=2e-5;
A=[-epsi 1 0;zeros(2,1) Areduced];
C=[1 0 0]; %measurement of the "integral".
G=[0 1;Greduced zeros(2,1)]; %noise in integral added as input.
sys_with_integrating_sensor=ss(A,G,C,0); %AUGMENTED process

% Noise input PSD:

W_ad_sensor=4e-3; %measurement noise to integrator
W_process=1; %process noise, causing random acceleration.
W=diag([W_process W_ad_sensor]); %continuous-time PSD matrix.

% Discretization of the augmented process:

Ts=0.002;
Ad=expm(A*Ts);
eig(Ad)'
syms t T real
Wdsym=simplify(int(expm(A*t)*G*W*G'*expm(A'*t),0,T));
format long
Wd=eval(subs(Wdsym,T,Ts)) %There may be correlation process-measurement noise...

% Euler-like (Euler-Maruyama) approximation for fast samping rate (small T_s\to 0):

G*W*G'*Ts %variance for noise simulation at small Ts.

% Stationary covariance matrix of the states:

format short
P=lyap(A,G*W*G') %would be infinity for pure integrator...
P(2:3,2:3) %physical (position, speed) covariance
Pred=lyap(Areduced,Greduced*W_process*Greduced')

%% Simulation  (realization of stochastic process)
% Let us simulate the system to see what it looks like:

Nsim=round(60/Ts);
w=mvnrnd([0,0,0],Wd,Nsim);
Trg=(0:(Nsim-1))*Ts;
sysd=ss(Ad,eye(3),[0 1 0],0,Ts); %simulate to only "position", not "position integral"!
y=lsim(sysd,w,Trg,mvnrnd([0;0;0],P));
plot(Trg,y), grid on, xlabel("Time"), ylabel("position")

%% Position estimation via averaging (finite-time integral)
% Stationary covariance between states at different times:
% cov(x(t+T_s),x(t))=e^{A\cdot T_s}P
% A vector [x(0); x(Ts)] will have covariance:

bigP=[P P*Ad';Ad*P P]

% If position were constant, this would be the optimal filter \frac{1}{T_s}\int_0^{T_s}pos(\tau)\,d\tau... We obtain it with a "finite difference" of the integrator state

FinDiff=1/Ts*[-1 1] %naive finite-difference

% The actual variance of this estimator is:

MM=[-1/Ts 0 0 1/Ts -1 0 ];
MM*bigP*MM'

%% Comparison with discrete-time Kalman filter
% We assume we "measure" the integrator state with basically "zero" variance every Ts:

sysd=ss(Ad,eye(3),[1 0 0],0,Ts);
[~,L,~,~,z2disc]=kalman(sysd,Wd,1e-12);

% The posterior error variance of the discrete-time Kalman filter is:

format long
z2disc
trace(z2disc)
format short

% The discrete-time Kalman filter gain is:

L

%% Converging to the continuous-time Kalman filter
% The continuous-time Kalman filter with the same info (3rd order system):

[~,Lc3,z2x]=kalman(sys_with_integrating_sensor,W,1e-12);
z2x
Lc3 %idealized case, clean measurements do NOT exist...

% We are multiplying an "infinitesimally small" result of the integrator by an "infinitely big" observer gain... This is not the way to go: let's use the NON-augmented sytem.
% The continuous-time Kalman filter with position sensor contaminated with, of course, white noise with same PSD, entering to other "electronics" given by the KF instead of "averaging integrator":

[~,Lc2,z2reduced]=kalman(sys_reduced,W_process,W_ad_sensor);
Lc2
format long
z2reduced
trace(z2reduced)
format short

%%
%% Optimal sampling period (for non-Kalman approach)
% Let us build a function so we can explore different sampling rates, copying the above code:

function [VVnaive,VVKal]=PredVariance(Ts,Data)
  P=Data.P;
  Ad=expm(Data.A*Ts);Bd=P-Ad*P*Ad';Bd=(Bd+Bd')/2;
  bigP=[P P*Ad';Ad*P P];
  Gnaive=[-1/Ts 0 0 1/Ts -1 0];
  VVnaive=Gnaive*bigP*Gnaive';
  sysd=ss(Ad,eye(3),[1 0 0],0,Ts);
  [~,~,~,~,z2]=kalman(sysd,Bd,1e-12);
  VVKal=z2(2,2);  
end
Data.P=P;Data.A=A;

function Tsbest=PlotThings(Data)
Tsrange=logspace(-5,3,100);
N=length(Tsrange);
VVnaiveplot=zeros(1,N);
VVKalmanplot=zeros(1,N);
for k=1:N
    [VVnaiveplot(k),VVKalmanplot(k)]=PredVariance(Tsrange(k),Data);
end
loglog(Tsrange,[1./Tsrange*4e-3;VVnaiveplot; VVKalmanplot],LineWidth=2), grid on,
yline(Data.P(2,2),':',LineWidth=2) 
ylim([1e-3 Data.P(2,2)*2])
xlabel("log Ts"),ylabel("log Prediction error variance (position)")
[~,idxbest]=min(VVnaiveplot);
Tsbest=Tsrange(idxbest);
end

Tsbest=PlotThings(Data)
yline(z2reduced(1,1), 'g-.',LineWidth=2) %
legend("ideal finite diff variance if constant","Finite Diff actual variance","Kalman Discr. order 3","prior position variance","Kalman Continous Time",Location="best")
hold off