clear
close all

A=1;
H=1;
%process covariance
Q=0.00001;
%measurement covariance
R=0.1;
xhat=3;
P=10;

%initial state
cnt=1
x(cnt)=3;
%u control inputs (such as steering commands in a vehicle)
P(cnt)=1;
z(cnt)=1;
V=1.5;

numobs=100;
cntindx=[1:numobs];
%how does one relate stnd dev of meas errors to meas covariance?
stddevnom=V*R;
%make suite of measurements with specified statistics
z=V+stddevnom*randn(1,numobs);

for cnt=2:1:numobs

%x_pred=A*x(cnt-1)+B*u(cnt); %no inputs u, so B=0 - leave out
x_pred=A*x(cnt-1);
P_pred=A*P(cnt-1)*A'+Q;

y=z(cnt)-H*x_pred;
S=H*P_pred*H'+R;
K=P_pred*H'*inv(S);

%outputs
x(cnt)=x_pred+K*y;
P(cnt)=(eye(1,1)-K*H)*P_pred;

end

averg=mean(z)
stddev=std(z)
runaverg=cumsum(z)./cntindx;
dif_kf_ra=x-runaverg;

val=[V V];
valx=[1 length(z)];

plot(cntindx,z,'Color',[.85 .85 .85])
hold
plot(cntindx,runaverg,'o',cntindx,z,'+g',cntindx,x,'*r',valx,val,'k',valx,val-stddev,'--k',valx,val+stddev,'--k');
hleg1=legend('data', 'running ave','data','kalman estimate','value', '+/- sigma','Location','NorthWest');
title('Kalman filter example - Nominal value 1.5, stnd dev .15')
xlabel('trial')
ylabel('value')

new_axis = axes('position',[ 0.55 0.6 0.3 0.3]);
set(new_axis,'color','none');
hist(z);
xlabel('meas value')
ylabel('num obs')