wolffd@0: % This is like robot1, except we only use a Kalman filter.
wolffd@0: % The goal is to study how the precision matrix changes.
wolffd@0: 
wolffd@0: seed = 1;
wolffd@0: rand('state', seed);
wolffd@0: randn('state', seed);
wolffd@0: 
wolffd@0: if 0
wolffd@0:   T = 20;
wolffd@0:   ctrl_signal = [repmat([1 0]', 1, T/4) repmat([0 1]', 1, T/4) ...
wolffd@0: 		 repmat([-1 0]', 1, T/4) repmat([0 -1]', 1, T/4)];
wolffd@0: else
wolffd@0:   T = 60;
wolffd@0:   ctrl_signal = repmat([1 0]', 1, T);
wolffd@0: end
wolffd@0: 
wolffd@0: nlandmarks = 6;
wolffd@0: if 0
wolffd@0:   true_landmark_pos = [1 1;
wolffd@0: 		    4 1;
wolffd@0: 		    4 4;
wolffd@0: 		    1 4]';
wolffd@0: else
wolffd@0:   true_landmark_pos = 10*rand(2,nlandmarks);
wolffd@0: end
wolffd@0: if 0
wolffd@0: figure(1); clf
wolffd@0: hold on
wolffd@0: for i=1:nlandmarks
wolffd@0:   %text(true_landmark_pos(1,i), true_landmark_pos(2,i), sprintf('L%d',i));
wolffd@0:   plot(true_landmark_pos(1,i), true_landmark_pos(2,i), '*')
wolffd@0: end
wolffd@0: hold off
wolffd@0: end
wolffd@0: 
wolffd@0: init_robot_pos = [0 0]';
wolffd@0: 
wolffd@0: true_robot_pos = zeros(2, T);
wolffd@0: true_data_assoc = zeros(1, T);
wolffd@0: true_rel_dist = zeros(2, T);
wolffd@0: for t=1:T
wolffd@0:   if t>1
wolffd@0:     true_robot_pos(:,t) = true_robot_pos(:,t-1) + ctrl_signal(:,t);
wolffd@0:   else
wolffd@0:     true_robot_pos(:,t) = init_robot_pos + ctrl_signal(:,t);
wolffd@0:   end
wolffd@0:   nn = argmin(dist2(true_robot_pos(:,t)', true_landmark_pos'));
wolffd@0:   %true_data_assoc(t) = nn;
wolffd@0:   %true_data_assoc = wrap(t, nlandmarks); % observe 1, 2, 3, 4, 1, 2, ...
wolffd@0:   true_data_assoc  = sample_discrete(normalise(ones(1,nlandmarks)),1,T);
wolffd@0:   true_rel_dist(:,t) = true_landmark_pos(:, nn) - true_robot_pos(:,t);
wolffd@0: end
wolffd@0: 
wolffd@0: R = 1e-3*eye(2); % noise added to observation
wolffd@0: Q = 1e-3*eye(2); % noise added to robot motion
wolffd@0: 
wolffd@0: % Create data set
wolffd@0: obs_noise_seq = sample_gaussian([0 0]', R, T)';
wolffd@0: obs_rel_pos = true_rel_dist + obs_noise_seq;
wolffd@0: %obs_rel_pos = true_rel_dist;
wolffd@0: 
wolffd@0: 
wolffd@0: %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
wolffd@0: % Create params for inference
wolffd@0: 
wolffd@0: % X(t) = A X(t-1) + B U(t) + noise(Q) 
wolffd@0: 
wolffd@0: % [L1]  = [1     ]  * [L1]       + [0]  * Ut  + [0   ]
wolffd@0: % [L2]    [  1   ]    [L2]         [0]          [ 0  ]
wolffd@0: % [R ]t   [     1]    [R ]t-1      [1]          [   Q]
wolffd@0: 
wolffd@0: % Y(t)|S(t)=s  = C(s) X(t) + noise(R)
wolffd@0: % Yt|St=1 = [1 0 -1]  * [L1]  + R
wolffd@0: %                       [L2]    
wolffd@0: %                       [R ]    
wolffd@0: 
wolffd@0: % Create indices into block structure
wolffd@0: bs = 2*ones(1, nlandmarks+1); % sizes of blocks in state space
wolffd@0: robot_block =  block(nlandmarks+1, bs);
wolffd@0: for i=1:nlandmarks
wolffd@0:   landmark_block(:,i) = block(i, bs)';
wolffd@0: end
wolffd@0: Xsz = 2*(nlandmarks+1); % 2 values for each landmark plus robot
wolffd@0: Ysz = 2; % observe relative location
wolffd@0: Usz = 2; % input is (dx, dy)
wolffd@0: 
wolffd@0: 
wolffd@0: % create block-diagonal trans matrix for each switch
wolffd@0: A = zeros(Xsz, Xsz);
wolffd@0: for i=1:nlandmarks
wolffd@0:   bi = landmark_block(:,i);
wolffd@0:   A(bi, bi) = eye(2);
wolffd@0: end
wolffd@0: bi = robot_block;
wolffd@0: A(bi, bi) = eye(2);
wolffd@0: A = repmat(A, [1 1 nlandmarks]); % same for all switch values
wolffd@0: 
wolffd@0: % create block-diagonal system cov
wolffd@0: 
wolffd@0: 
wolffd@0: Qbig = zeros(Xsz, Xsz);
wolffd@0: bi = robot_block;
wolffd@0: Qbig(bi,bi) = Q; % only add noise to robot motion
wolffd@0: Qbig = repmat(Qbig, [1 1 nlandmarks]);
wolffd@0: 
wolffd@0: % create input matrix
wolffd@0: B = zeros(Xsz, Usz);
wolffd@0: B(robot_block,:) = eye(2); % only add input to robot position
wolffd@0: B = repmat(B, [1 1 nlandmarks]);
wolffd@0: 
wolffd@0: % create observation matrix for each value of the switch node
wolffd@0: % C(:,:,i) = (0 ... I ... -I) where the I is in the i'th posn.
wolffd@0: % This computes L(i) - R
wolffd@0: C = zeros(Ysz, Xsz, nlandmarks);
wolffd@0: for i=1:nlandmarks
wolffd@0:   C(:, landmark_block(:,i), i) = eye(2); 
wolffd@0:   C(:, robot_block, i) = -eye(2);
wolffd@0: end
wolffd@0: 
wolffd@0: % create observation cov for each value of the switch node
wolffd@0: Rbig = repmat(R, [1 1 nlandmarks]);
wolffd@0: 
wolffd@0: % initial conditions
wolffd@0: init_x = zeros(Xsz, 1);
wolffd@0: init_v = zeros(Xsz, Xsz);
wolffd@0: bi = robot_block;
wolffd@0: init_x(bi) = init_robot_pos;
wolffd@0: %init_V(bi, bi) = 1e-5*eye(2); % very sure of robot posn
wolffd@0: init_V(bi, bi) = Q; % simualate uncertainty due to 1 motion step
wolffd@0: for i=1:nlandmarks
wolffd@0:   bi = landmark_block(:,i);
wolffd@0:   init_V(bi,bi)= 1e5*eye(2); % very uncertain of landmark psosns
wolffd@0:   %init_x(bi) = true_landmark_pos(:,i);
wolffd@0:   %init_V(bi,bi)= 1e-5*eye(2); % very sure of landmark psosns
wolffd@0: end
wolffd@0: 
wolffd@0: %k = nlandmarks-1; % exact
wolffd@0: k = 3;
wolffd@0: ndx = {};
wolffd@0: for t=1:T
wolffd@0:   landmarks = unique(true_data_assoc(t:-1:max(t-k,1)));
wolffd@0:   tmp = [landmark_block(:, landmarks) robot_block'];
wolffd@0:   ndx{t} = tmp(:);
wolffd@0: end
wolffd@0: 
wolffd@0: [xa, Va] = kalman_filter(obs_rel_pos, A, C, Qbig, Rbig, init_x, init_V, ...
wolffd@0: 				     'model', true_data_assoc, 'u', ctrl_signal, 'B', B, ...
wolffd@0: 		       'ndx', ndx);
wolffd@0: 
wolffd@0: [xe, Ve] = kalman_filter(obs_rel_pos, A, C, Qbig, Rbig, init_x, init_V, ...
wolffd@0: 				     'model', true_data_assoc, 'u', ctrl_signal, 'B', B);
wolffd@0: 
wolffd@0: 
wolffd@0: if 0
wolffd@0: est_robot_pos = x(robot_block, :);
wolffd@0: est_robot_pos_cov = V(robot_block, robot_block, :);
wolffd@0: 
wolffd@0: for i=1:nlandmarks
wolffd@0:   bi = landmark_block(:,i);
wolffd@0:   est_landmark_pos(:,i) = x(bi, T);
wolffd@0:   est_landmark_pos_cov(:,:,i) = V(bi, bi, T);
wolffd@0: end
wolffd@0: end
wolffd@0: 
wolffd@0: 
wolffd@0: 
wolffd@0: nrows = 10;
wolffd@0: stepsize = T/(2*nrows);
wolffd@0: ts = 1:stepsize:T;
wolffd@0: 
wolffd@0: if 1 % plot
wolffd@0:   
wolffd@0: clim = [0 max(max(Va(:,:,end)))];
wolffd@0: 
wolffd@0: figure(2)
wolffd@0: if 0
wolffd@0:   imagesc(Ve(1:2:end,1:2:end, T))
wolffd@0:   clim = get(gca,'clim');
wolffd@0: else
wolffd@0:   i = 1;
wolffd@0:   for t=ts(:)'
wolffd@0:     subplot(nrows,2,i)
wolffd@0:     i = i + 1;
wolffd@0:     imagesc(Ve(1:2:end,1:2:end, t))
wolffd@0:     set(gca, 'clim', clim)
wolffd@0:     colorbar
wolffd@0:   end
wolffd@0: end
wolffd@0: suptitle('exact')
wolffd@0: 
wolffd@0: 
wolffd@0: figure(3)
wolffd@0: if 0
wolffd@0:   imagesc(Va(1:2:end,1:2:end, T))
wolffd@0:   set(gca,'clim', clim)
wolffd@0: else
wolffd@0:   i = 1;
wolffd@0:   for t=ts(:)'
wolffd@0:     subplot(nrows,2,i)
wolffd@0:     i = i+1;
wolffd@0:     imagesc(Va(1:2:end,1:2:end, t))
wolffd@0:     set(gca, 'clim', clim)
wolffd@0:     colorbar
wolffd@0:   end
wolffd@0: end
wolffd@0: suptitle('approx')
wolffd@0: 
wolffd@0: 
wolffd@0: figure(4)
wolffd@0: i = 1;
wolffd@0: for t=ts(:)'
wolffd@0:   subplot(nrows,2,i)
wolffd@0:   i = i+1;
wolffd@0:   Vd = Va(1:2:end,1:2:end, t) - Ve(1:2:end,1:2:end,t);
wolffd@0:   imagesc(Vd)
wolffd@0:   set(gca, 'clim', clim)
wolffd@0:   colorbar
wolffd@0: end
wolffd@0: suptitle('diff')
wolffd@0: 
wolffd@0: end % all plot
wolffd@0: 
wolffd@0: 
wolffd@0: for t=1:T
wolffd@0:   i = 1:2*nlandmarks;
wolffd@0:   denom = Ve(i,i,t) + (Ve(i,i,t)==0);
wolffd@0:   Vd =(Va(i,i,t)-Ve(i,i,t)) ./ denom;
wolffd@0:   Verr(t) = max(Vd(:));
wolffd@0: end
wolffd@0: figure(6); plot(Verr)
wolffd@0: title('max relative Verr')
wolffd@0: 
wolffd@0: for t=1:T
wolffd@0:   %err(t)=rms(xa(:,t), xe(:,t));
wolffd@0:   err(t)=rms(xa(1:end-2,t), xe(1:end-2,t)); % exclude robot
wolffd@0: end
wolffd@0: figure(5);plot(err)
wolffd@0: title('rms mean pos')