% Generate true path
p1 = [-1*ones(1,40) -1:1/20:1 1*ones(1,41)];
p2 = [-1:1/20:1 1*ones(1,40) 1:-1/20:-1];

% Generate noisy path measurements
y1 = p1 + 0.2*randn(1,122);
y2 = p2 + 0.2*randn(1,122);

% Plot true path and measurements
clf
plot(p1,p2,'k-','Linew',3);
hold on
plot(y1,y2,'b--+','Linew',1)
axis equal
axis([-1.8 1.8 -1.8 1.8])
grid on
xlabel('y_1')
ylabel('y_2')

% Design Kalman filter for two parallel double integrators
A = [0 1; 0 0]; A = blkdiag(A,A);
G = [0; 1]; G = blkdiag(G,G);
C = [1 0]; C = blkdiag(C,C);
R1 = 10000*eye(2);                % Process noise
R2 = 10000*eye(2);                % Measurement noise
K = lqe(A,G,C,R1,R2);       % Kalman filter gain
sysk = ss(A,G,C,0);         % Form observer system
obs = estim(sysk,K);      

% Simulate observer and plot result
t = 0:length(y1)-1;
yhat = lsim(obs,[y1;y2],t); 
plot(yhat(:,1),yhat(:,2),'r-o','Linew',1.5)
