clear
close all

%calc exact

%set up vector of time steps, this also controls lengths of all the other vectors
dt=.1;
t=[0:dt:20]';
numobs=length(t);

%set up "input" - force of gravity on object
g=-9.81;
gx=0;
gy=g;

%set up vectors for exact physics calculation, will need these to show exact solution and to generate noisy datat for input to KF
x=zeros(length(t),1);
y=x;
vx=x;
vy=x;

%to fill in and pad matricies
ceros=x';
unos=ceros+1;

%initial conditions - velocity and angle
v=100;
ang=45;
vx(1)=v*cos(pi*ang/180);
vy(1)=v*sin(pi*ang/180);

%calc physics solution
x=x+vx(1)*t+0.5*gx*t.^2;
y=y+vy(1)*t+0.5*gy*t.^2;
vx=vx(1)+gx*t;
vy=vy(1)+gy*t;

plot(x,y,'k')
axis equal
grid

%now do kf

%set up kf matricies
A=[1 dt 0 0; 0 1 0 0; 0 0 1 dt; 0 0 0 1];
B=[0 0 0 0; 0 0 0 0; 0 0 1 0; 0 0 0 1];
u=[ceros; ceros; unos*0.5*g*dt^2; unos*g*dt];

%H converts measurements to state parameters, in this case measurements map directly into state parameters
H=eye(4);
%process covariance - since we know the physics this is zero (how to
%estimate is one of the "tricky" parts of kf (e.g. - we are not including air
%resistance in our physics, if important and effects the measurements would
%need something else here)
Q=zeros(4);
%measurement covariance - again one of the "tricky" parts of kf, assume all
%same value and independent
r=0.4;
R=r*eye(4);
%initial guess for covarience - another "tricky" part of kf
P(:,:,1)=eye(4);

%initial state
xerr=100;
yerr=300;

cnt=1

%initialize kf state variables
Xkf(:,cnt)=[x(1)+xerr vx(1) y(1)+yerr vy(1)]';

%how does one relate stnd dev of meas errors to meas covariance?
stddevnom=r*v;
%make suite of measurements with specified statistics (we are measuring
%position and velocity - could re-formulate with only position measurements)
Zx=(x+stddevnom*randn(numobs,1));
Zy=(y+stddevnom*randn(numobs,1));
Zvx=(vx+stddevnom*randn(numobs,1));
Zvy=(vy+stddevnom*randn(numobs,1));
Z=[Zx Zvx Zy Zvy]';

hold
plot(Zx,Zy,'--+')

%now apply kf to estimate position
for cnt=2:1:numobs

%calc predicted position from previous state and physics
X_pred=A*Xkf(:,cnt-1)+B*u(:,cnt);
%calc predicted covariance
P_pred=A*P(:,:,cnt-1)*A'+Q;

%calculate "innovation" - the difference between "reality" (based on measurements) and prediction
Y(:,cnt)=Z(:,cnt)-H*X_pred;
%calc innovation covarience
S=H*P_pred*H'+R;
%calc kalman "gain", H*inv(S) converts measurements to state parameters 
K=P_pred*H'*inv(S);

%outputs - adjust position and velocity using predicted value plus some
%part (weighted by K) of the "innovation" (the measured - predicted error)
Xkf(:,cnt)=X_pred+K*Y(:,cnt);
%calc new covariance from kalman gain and predicted covariance 
P(:,:,cnt)=(eye(4)-K*H)*P_pred;

end

plot(Xkf(1,:),Xkf(3,:),'r')

%hleg1=legend('data', 'running ave','data','kalman estimate','value', '+/- sigma','Location','NorthWest');
title('Kalman filter example')
xlabel('x position')
ylabel('y position')