%%% Implementation of (Univariate) Kalman Filter
%[squarederror] = univkalman(K, phi,d,n,T)

K=0.01;
phi=0.5 % the dynamics of the uncontrolled system
d=10 % the mean of of the environmental noise
n=0.2 % mean of the observation noise
T=40 % number of timesteps

%% Dynamics of system
y=5; % initial conditions
nu=5; % initial conditions
ybestest=10; % initial conditions

u=randn(T,1) % my controls - could just suppose these are a set of known (but iid Gaussian) commands 
dnoise=d*randn(T,1) % environmental noise process
nnoise=n*randn(T,1)  % observation noise process

for time=0:T-1 % we'll present this as a loop for interpretability
    
 %% here we present the actual dynamics
    
new = phi*y(time+1) + u(time+1) + dnoise(time+1)
y=[y new]

%% the observations of the system polluted with observation noise
newnu=new + nnoise(time+1)
nu= [nu newnu]

%% prediction given past estimate and dynamics
predict= phi*ybestest(time+1) + u(time+1)

%% bestestimate
bestest=(1-K)*predict+K*newnu

%% update of bestestimates
ybestest=[ybestest bestest]

end

squarederror= sum((ybestest-y).*(ybestest-y))

figure; plot(y,'Color','red')
hold on
plot(ybestest,'Color','g')
hold on
plot(nu)
legend('actual','bestestimate','observation')

xlabel('Time')
ylabel('y, yk|k, nu')