├── repvec.m ├── randstates.mat ├── pi_to_pi.m ├── robot_motion_model.m ├── robot_measurement_model.m ├── setdiff_ns.m ├── README.md ├── isspd.m ├── gen_map.m ├── prop_jacob_ut.m ├── meas_jacob_ut.m ├── plot_all.m ├── rws.m ├── bmap.m ├── main.m ├── uis.m └── isam.m /repvec.m: -------------------------------------------------------------------------------- 1 | function x = repvec(x,N) 2 | x = x(:, ones(1,N)); -------------------------------------------------------------------------------- /randstates.mat: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/rpng/clatt/HEAD/randstates.mat -------------------------------------------------------------------------------- /pi_to_pi.m: -------------------------------------------------------------------------------- 1 | function angle = pi_to_pi(angle) 2 | 3 | angle = atan2(sin(angle), cos(angle)); -------------------------------------------------------------------------------- /robot_motion_model.m: -------------------------------------------------------------------------------- 1 | function [y] = robot_motion_model(x,dt) 2 | 3 | for i = 1:size(x,2) 4 | 5 | v = x(end-1,i); 6 | omega = x(end,i); 7 | 8 | y(:,i) = x(1:end-2,i); 9 | 10 | y(1:3,i) = [ x(1,i) + v*dt*cos(x(3,i)); 11 | x(2,i) + v*dt*sin(x(3,i)); 12 | pi_to_pi(x(3,i) + omega*dt) ]; 13 | 14 | end 15 | -------------------------------------------------------------------------------- /robot_measurement_model.m: -------------------------------------------------------------------------------- 1 | function [zhat] = robot_measurement_model(x) 2 | 3 | fpos = 3 + 1; 4 | 5 | for i = 1:size(x,2) 6 | 7 | C = [cos(x(3,i)) -sin(x(3,i)); sin(x(3,i)) cos(x(3,i)) ]; 8 | 9 | k_xL = C'*(x(fpos:fpos+1,i)-x(1:2,i)); 10 | 11 | rho = norm(k_xL); 12 | th = atan2(k_xL(2),k_xL(1)); 13 | zhat(:,i) = [rho; th]; 14 | 15 | end -------------------------------------------------------------------------------- /setdiff_ns.m: -------------------------------------------------------------------------------- 1 | function [A] = setdiff_ns(A,B) 2 | % Returns the values in A that are not in B, preserving the order of A. 3 | % A and B are expected to be row vectors. 4 | 5 | A = A(~ismembc(A,sort(B))); 6 | 7 | if ~isempty(A) 8 | [T,R] = sort(A); 9 | I = [true diff(T)~=0]; 10 | T = T(I); 11 | R = R(I); 12 | [I,I] = sort(R); 13 | A = T(I); 14 | end -------------------------------------------------------------------------------- /README.md: -------------------------------------------------------------------------------- 1 | # CLATT 2 | Cooperative Localization and Target Tracking in 2D 3 | 4 | # Reference 5 | Guoquan Huang, Michael Kaess and John Leonard, Consistent Unscented Incremental Smoothing for Multi-robot Cooperative Target Tracking, Robotics and Autonomous Systems, vol. 69, pp. 52-67, July 2015. 6 | 7 | # Notes 8 | - SuiteSparse is required: http://faculty.cse.tamu.edu/davis/suitesparse.html 9 | - Change simulation parameter as needed in the main script (main.m) 10 | - Run main.m 11 | - This is proof-of-concept implementation which by no means is optimized. -------------------------------------------------------------------------------- /isspd.m: -------------------------------------------------------------------------------- 1 | 2 | function [t,R] = isspd(Sigma) 3 | %ISPDS Test if a matrix is positive definite symmetric 4 | % T = ISPDS(SIGMA) returns a logical indicating whether the matrix SIGMA is 5 | % square, symmetric, and positive definite, i.e., it is a valid full rank 6 | % covariance matrix. 7 | % 8 | % [T,R] = ISPDS(SIGMA) returns the cholesky factor of SIGMA in R. If SIGMA 9 | % is not square symmetric, ISPDS returns [] in R. 10 | 11 | % Test for square, symmetric 12 | % 13 | [nSamples,m] = size(Sigma); 14 | if (nSamples == m) & all(all(abs(Sigma - Sigma') < 10*eps(max(abs(diag(Sigma)))))) 15 | % Test for positive definiteness 16 | [R,p] = chol(Sigma); 17 | if p == 0 18 | t = true; 19 | else 20 | t = false; 21 | end 22 | else 23 | R = []; 24 | t = false; 25 | end -------------------------------------------------------------------------------- /gen_map.m: -------------------------------------------------------------------------------- 1 | function xL = gen_map(nL,v,omega,rmin,rmax, nSteps,dt) 2 | 3 | if omega~=0 4 | 5 | % radius 6 | r = v/omega; 7 | 8 | for i=1:nL 9 | 10 | if rand > .5 %outside of circle 11 | disp('landmarks: ourside of circle') 12 | rho = r + rmin + (rmax-rmin)*rand; 13 | th = 2*pi*rand; 14 | else %inside of circle 15 | disp('landmarks: inside of circle') 16 | rho = r - rmax + (rmax-rmin)*rand; 17 | th = 2*pi*rand; 18 | end 19 | 20 | [x,y] = pol2cart(th,rho); 21 | xL(:,i) = [x;y+r]; %shift y w/ r since robot starts at (0,0) 22 | 23 | end 24 | 25 | end 26 | % 27 | xL(:,1) = [0;r];%one landmark at center 28 | 29 | 30 | % % %exploration case 31 | if omega==0 32 | 33 | for i=1:nL 34 | if round(rand) 35 | rho = rmin + (rmax-rmin)*rand; 36 | th = 2*pi*rand; 37 | else 38 | rho = - rmax + (rmax-rmin)*rand; 39 | th = 2*pi*rand; 40 | end 41 | [x,y] = pol2cart(th,rho); 42 | yy(1,i) = rho;%y; 43 | end 44 | 45 | dtraj = v*dt*nSteps; 46 | 47 | xL = [dtraj*rand(1,nL); yy ]; 48 | 49 | 50 | end 51 | -------------------------------------------------------------------------------- /prop_jacob_ut.m: -------------------------------------------------------------------------------- 1 | function [PHI, sqrtQ,yhat] = prop_jacob_ut(fmotion,x, xlin, P,v,omega,Q,dt, doOC) 2 | 3 | % sample entire state 4 | x = [x; v;omega]; % 5 | P = blkdiag(P,Q); % 6 | 7 | dim = length(x); 8 | npnts = dim*2 + 1; 9 | scale = 3; %scale = dim+lambda 10 | kappa = scale-dim; 11 | 12 | % generate samples 13 | Pi = chol(P)' * sqrt(scale); 14 | xi = [x, repvec(x,dim)+Pi, repvec(x,dim)-Pi]; 15 | 16 | % transform the samples through measuremnt function 17 | yi = feval(fmotion,xi,dt); 18 | y = repvec(yi(:,1),npnts); % set first transformed sample as the base 19 | ri = y - yi; 20 | ri(3,:) = pi_to_pi(ri(3,:)); % compute correct residual 21 | yi = y - ri; % offset yi from base according to correct residual 22 | 23 | yhat = (kappa*yi(:,1) + 0.5*sum(yi(:,2:end),2)) / scale; 24 | 25 | dx = xi-repvec(x,npnts); 26 | dy = yi-repvec(yhat,npnts); 27 | 28 | Pxy = (2*kappa*dx(:,1)*dy(:,1)' + dx(:,2:end)*dy(:,2:end)') / (2*scale); 29 | Pyy = (2*kappa*dy(:,1)*dy(:,1)' + dy(:,2:end)*dy(:,2:end)') / (2*scale); 30 | 31 | F = (P\Pxy)'; 32 | 33 | Pee = Pyy - F*P*F'; %Qstar 34 | if min(eig(Pee))<1e-6 35 | Pee = Pee + eye(size(Pee))*1e-6; 36 | end 37 | sqrtQ = sqrtm(eye(size(Pee))/Pee); 38 | 39 | 40 | % correspondence to the normal ekf propagation 41 | PHI = F(:,1:end-2); 42 | G = F(:,end-1:end); 43 | 44 | % % 45 | if doOC 46 | J = [0 -1; 1 0]; 47 | PHIproj = [ PHI, - eye(3)] ; 48 | % %construct nullspace 49 | N = [ eye(2), J*xlin(1:2,1); 0 0 1 ; eye(2), J*xlin(4:5,1); 0 0 1 ]; 50 | % % projection 51 | PHIproj = PHIproj * (eye(size(xlin,1)) - N/(N'*N)*N') ; 52 | PHI = PHIproj(:,1:3); 53 | end 54 | 55 | 56 | -------------------------------------------------------------------------------- /meas_jacob_ut.m: -------------------------------------------------------------------------------- 1 | function [r, H1,H2] = meas_jacob_ut(hmeas, x, xlin,P, zm, R, doOC) 2 | 3 | J = [0 -1; 1 0]; 4 | dim = length(x); 5 | npnts = dim*2 + 1; 6 | scale = 3; % scale = dim+lambda 7 | kappa = scale-dim; 8 | 9 | % generate samples 10 | Pi = chol(P)' * sqrt(scale); 11 | xi = [x, repvec(x,dim)+Pi, repvec(x,dim)-Pi]; 12 | 13 | % transform the samples through measuremnt function 14 | zi = feval(hmeas,xi); 15 | z = repvec(zi(:,1),npnts); % set first transformed sample as the base 16 | ri = z-zi; 17 | ri(2,:) = pi_to_pi(ri(2,:)); % compute correct residual 18 | zi = z - ri; % offset zi from base according to correct residual 19 | 20 | global gRB 21 | if gRB==3 22 | zhat = (kappa*zi(:,1) + 0.5*sum(zi(:,2:end), 2)) / scale; 23 | r = [ zm(1)-zhat(1,1); pi_to_pi(zm(2)-zhat(2,1)) ] ; %beware of sign 24 | elseif gRB==2 25 | zi = zi(2,:); %bearing only 26 | zhat = (kappa*zi(:,1) + 0.5*sum(zi(:,2:end), 2)) / scale; 27 | r = pi_to_pi(zm(2)-zhat) ; %beware of sign 28 | elseif gRB==1 29 | zi = zi(1,:); %range only 30 | zhat = (kappa*zi(:,1) + 0.5*sum(zi(:,2:end), 2)) / scale; 31 | r = zm(1)-zhat; %beware of sign 32 | end 33 | 34 | dx = xi - repvec(x,npnts); 35 | dz = zi - repvec(zhat,npnts); 36 | 37 | Pxz = (2*kappa*dx(:,1)*dz(:,1)' + dx(:,2:end)*dz(:,2:end)') / (2*scale); 38 | Pzz = (2*kappa*dz(:,1)*dz(:,1)' + dz(:,2:end)*dz(:,2:end)') / (2*scale); 39 | 40 | Hproj = Pxz' / P; 41 | 42 | % % 43 | if doOC 44 | % %construct nullspace % % 45 | if length(x)==6 %robot-to-robot 46 | N = [ eye(2), J*xlin(1:2,1); 0 0 1 ; eye(2), J*xlin(4:5,1); 0 0 1 ]; 47 | else %robot-to-target 48 | dim_target = length(x)-3; 49 | N = [ eye(2), J*xlin(1:2,1); 0 0 1 ; eye(2), J*xlin(4:5,1); zeros(dim_target-2,2), kron(eye((dim_target-2)/2) , J)*xlin(6:end,1) ]; 50 | end 51 | % % projection 52 | Hproj = Hproj * (eye(size(x,1)) - N/(N'*N)*N') ; 53 | end 54 | 55 | H1 = Hproj(:,1:3); 56 | H2 = Hproj(:,4:end); 57 | 58 | -------------------------------------------------------------------------------- /plot_all.m: -------------------------------------------------------------------------------- 1 | close all 2 | SIMorEXP = 1; 3 | start = n_init_steps+1; 4 | incr = 1; 5 | 6 | % % trajectory 7 | for irun = 1 %floor(rand*nRuns+.5)+1%1:nRuns 8 | 9 | figure, hold on 10 | 11 | if ~isempty(xL_true) 12 | plot(xL_true(1,:,irun), xL_true(2,:,irun), 'm+','Linewidth',2,'Markersize',10) 13 | end 14 | 15 | if nT>0, plot(squeeze(xT_true(1,1,:,irun)), squeeze(xT_true(2,1,:,irun)), 'g--o','Linewidth',1), end 16 | if nT>1, plot(squeeze(xT_true(1,2,:,irun)), squeeze(xT_true(2,2,:,irun)), 'm--s','Linewidth',1), end 17 | if nT>2, plot(squeeze(xT_true(1,3,:,irun)), squeeze(xT_true(2,3,:,irun)), 'c-->','Linewidth',1), end 18 | if nT>3, plot(squeeze(xT_true(1,4,:,irun)), squeeze(xT_true(2,4,:,irun)), 'y--d','Linewidth',1), end 19 | 20 | if nR>0,plot(squeeze(xR_true(1,1,:,irun)), squeeze(xR_true(2,1,:,irun)), 'r-','Linewidth',2),end 21 | if nR>1,plot(squeeze(xR_true(1,2,:,irun)), squeeze(xR_true(2,2,:,irun)), 'k--','Linewidth',2),end 22 | if nR>2,plot(squeeze(xR_true(1,3,:,irun)), squeeze(xR_true(2,3,:,irun)), 'b-.','Linewidth',2),end 23 | if nR>3,plot(squeeze(xR_true(1,4,:,irun)), squeeze(xR_true(2,4,:,irun)), 'c-+','Linewidth',1),end 24 | if nR>4,plot(squeeze(xR_true(1,5,:,irun)), squeeze(xR_true(2,5,:,irun)), 'm-*','Linewidth',1),end 25 | if nR>5,plot(squeeze(xR_true(1,6,:,irun)), squeeze(xR_true(2,6,:,irun)), 'y-x','Linewidth',1),end 26 | 27 | if ~isempty(xL_true) 28 | hh=legend('Landmarks', 'Target', 'Robot'); 29 | elseif nT==1 && nR==2 30 | hh=legend('Target 1', 'Robot 1','Robot 2'); 31 | elseif nT==2 && nR==4 32 | hh=legend('Target 1', 'Target 2', 'Robot 1','Robot 2','Robot 3','Robot 4'); 33 | elseif nT==3 && nR==6 34 | hh=legend('Target 1', 'Target 2','Target 3', 'Robot 1','Robot 2','Robot 3','Robot 4','Robot 5','Robot 6'); 35 | elseif nT==4 && nR==4 36 | hh=legend('Target 1', 'Target 2','Target 3', 'Target 4', 'Robot 1','Robot 2','Robot 3','Robot 4'); 37 | else 38 | % 39 | end 40 | 41 | xlabel('x (m)','FontWeight','bold','fontsize',12), ylabel('y (m)','FontWeight','bold','fontsize',12) 42 | 43 | if nR>0,plot(squeeze(xR_true(1,1,1,irun)), squeeze(xR_true(2,1,1,irun)), 'ro','Markersize',10,'Linewidth',2), end 44 | if nR>1,plot(squeeze(xR_true(1,2,1,irun)), squeeze(xR_true(2,2,1,irun)), 'ko','Markersize',10,'Linewidth',2),end 45 | if nR>2,plot(squeeze(xR_true(1,3,1,irun)), squeeze(xR_true(2,3,1,irun)), 'bo','Markersize',10,'Linewidth',2),end 46 | if nR>3,plot(squeeze(xR_true(1,4,1,irun)), squeeze(xR_true(2,4,1,irun)), 'co','Markersize',10,'Linewidth',2), end 47 | if nR>4,plot(squeeze(xR_true(1,5,1,irun)), squeeze(xR_true(2,5,1,irun)), 'mo','Markersize',10,'Linewidth',2), end 48 | 49 | if nT>0, plot(squeeze(xT_true(1,1,1,irun)), squeeze(xT_true(2,1,1,irun)), 'g>','Markersize',10,'Linewidth',2), end 50 | if nT>1, plot(squeeze(xT_true(1,2,1,irun)), squeeze(xT_true(2,2,1,irun)), 'm>','Markersize',10,'Linewidth',2),end 51 | if nT>2, plot(squeeze(xT_true(1,3,1,irun)), squeeze(xT_true(2,3,1,irun)), 'c>','Markersize',10,'Linewidth',2), end 52 | if nT>3, plot(squeeze(xT_true(1,4,1,irun)), squeeze(xT_true(2,4,1,irun)), 'y>','Markersize',10,'Linewidth',2), end 53 | 54 | axis equal 55 | 56 | if SIMorEXP 57 | print('-depsc2', ['traj_',int2str(irun)] ) 58 | else 59 | print('-depsc2', 'traj_exp' ) 60 | end 61 | % close 62 | end 63 | 64 | % % Robot RMS 65 | for ell = 1:nR 66 | figure 67 | subplot(2,1,1), hold on 68 | plot([start:incr:nSteps],squeeze(rmsRp_avg_isam2(ell,start:incr:end)),'g-.','Linewidth',2); 69 | plot([start:incr:nSteps],squeeze(rmsRp_avg_isam3(ell,start:incr:end)),'b-o','Linewidth',1); 70 | plot([start:incr:nSteps],squeeze(rmsRp_avg_bmap(ell,start:incr:end)),'r-','Linewidth',2); 71 | 72 | if SIMorEXP 73 | ylabel('Robot position RMSE (m)','FontWeight','bold','fontsize',12) 74 | else 75 | ylabel('Robot position error (m)','FontWeight','bold','fontsize',12) 76 | end 77 | 78 | hh = legend('Naive-UIS','UIS','MAP'); 79 | 80 | subplot(2,1,2), hold on 81 | 82 | plot([start:incr:nSteps],squeeze(rmsRth_avg_isam2(ell,start:incr:end)),'g-.','Linewidth',2); 83 | plot([start:incr:nSteps],squeeze(rmsRth_avg_isam3(ell,start:incr:end)),'b-o','Linewidth',1); 84 | plot([start:incr:nSteps],squeeze(rmsRth_avg_bmap(ell,start:incr:end)),'r-','Linewidth',2); 85 | 86 | 87 | if SIMorEXP 88 | xlabel('Time (sec)','FontWeight','bold','fontsize',12), 89 | ylabel('Robot heading RMSE (rad)','FontWeight','bold','fontsize',12) 90 | 91 | if dim_target==6 92 | print('-depsc2',['robot_rms_bo_',int2str(ell)]) 93 | else 94 | print('-depsc2',['robot_rms_',int2str(ell)]) 95 | end 96 | else 97 | xlabel('Time (sec)','FontWeight','bold','fontsize',12), 98 | ylabel('Robot heading error (rad)','FontWeight','bold','fontsize',12) 99 | print('-depsc2',['robot_rms_exp_',int2str(ell)]) 100 | end 101 | 102 | end 103 | 104 | RMS_Robot_Position = [ mean(rmsRp_avg_isam2(:,start:end),2), mean(rmsRp_avg_isam3(:,start:end),2), mean(rmsRp_avg_bmap(:,start:end),2) ] 105 | RMS_Robot_Heading = [ mean(rmsRth_avg_isam2(:,start:end),2), mean(rmsRth_avg_isam3(:,start:end),2), mean(rmsRth_avg_bmap(:,start:end),2) ] 106 | 107 | 108 | % % Target RMS 109 | for ell = 1:nT 110 | figure 111 | subplot(2,1,1), hold on 112 | 113 | plot([start:incr:nSteps],squeeze(rmsT_avg_isam2(ell,start:incr:end)),'g-.','Linewidth',2); 114 | plot([start:incr:nSteps],squeeze(rmsT_avg_isam3(ell,start:incr:end)),'b-o','Linewidth',1); 115 | plot([start:incr:nSteps],squeeze(rmsT_avg_bmap(ell,start:incr:end)),'r-','Linewidth',2); 116 | 117 | ylim([0 5]) 118 | 119 | if SIMorEXP 120 | ylabel('Target position RMSE (m)','FontWeight','bold','fontsize',12) 121 | else 122 | ylabel('Target position error (m)','FontWeight','bold','fontsize',12) 123 | end 124 | 125 | hh = legend('Naive-UIS','UIS','MAP'); 126 | 127 | subplot(2,1,2), hold on 128 | 129 | plot([start:incr:nSteps],squeeze(rmsTv_avg_isam2(ell,start:incr:end)),'g-.','Linewidth',2); 130 | plot([start:incr:nSteps],squeeze(rmsTv_avg_isam3(ell,start:incr:end)),'b-o','Linewidth',1); 131 | plot([start:incr:nSteps],squeeze(rmsTv_avg_bmap(ell,start:incr:end)),'r-','Linewidth',2); 132 | 133 | ylim([0 .6]) 134 | 135 | if SIMorEXP 136 | xlabel('Time (sec)','FontWeight','bold','fontsize',12), 137 | ylabel('Target velocity RMSE (m/sec)','FontWeight','bold','fontsize',12) 138 | if dim_target==6 139 | print('-depsc2',['target_rms_bo_',int2str(ell)]) 140 | else 141 | print('-depsc2',['target_rms_',int2str(ell)]) 142 | end 143 | else 144 | 145 | xlabel('Time (sec)','FontWeight','bold','fontsize',12), 146 | ylabel('Target velocity error (m/sec)','FontWeight','bold','fontsize',12) 147 | print('-depsc2',['target_rms_exp_',int2str(ell)]) 148 | end 149 | end 150 | 151 | RMS_Target_Position = [ mean(rmsT_avg_isam2(:,start:end),2),mean(rmsT_avg_isam3(:,start:end),2), mean(rmsT_avg_bmap(:,start:end),2) ] 152 | RMS_Target_Vel = [ mean(rmsTv_avg_isam2(:,start:end),2),mean(rmsTv_avg_isam3(:,start:end),2), mean(rmsTv_avg_bmap(:,start:end),2) ] 153 | 154 | 155 | -------------------------------------------------------------------------------- /rws.m: -------------------------------------------------------------------------------- 1 | %% Real-world simulation 2 | function [v_m,omega_m,v,omega,xR_true,zr,Rr, zl,Rl, xT_true,PHI,Qd,zt,Rt ] = rws(nR,nSteps, nL,xL_true, dt, ... 3 | v_true,omega_true,sigma_v,sigma_w, sigma_r,sigma_th,sigma_p, ... 4 | nT, vt, sigma_a, at, sigma_j,dim_target, ... 5 | max_range,min_range, r_max,omega_max,DORANDOM,SIGPERCENT) 6 | 7 | 8 | %% robots' starting poses 9 | x0 = zeros(3,nR); 10 | xyinit = 0; 11 | if nR>0, x0(:,1) = [xyinit, xyinit, 0]; end 12 | if nR>1, x0(:,2) = [xyinit, -xyinit, 0]; end 13 | if nR>2, x0(:,3) = [-xyinit, -xyinit, 0]; end 14 | if nR>3, x0(:,4) = [-xyinit, xyinit, 0]; end 15 | 16 | 17 | %% generate robot odometry 18 | for ell = 1:nR 19 | 20 | %true velocities w/ noise at each nSteps 21 | v(ell,:) = v_true(ell,:); 22 | omega(ell,:) = omega_true(ell,:); 23 | 24 | for k = 1:nSteps 25 | if k==1 26 | xR_true(:,ell,k) = x0(:,ell); 27 | else 28 | xR_true(1,ell,k) = xR_true(1,ell,k-1)+v(ell,k-1)*dt*cos(xR_true(3,ell,k-1)); 29 | xR_true(2,ell,k) = xR_true(2,ell,k-1)+v(ell,k-1)*dt*sin(xR_true(3,ell,k-1)); 30 | xR_true(3,ell,k) = pi_to_pi(xR_true(3,ell,k-1)+omega(ell,k-1)*dt); 31 | 32 | if DORANDOM 33 | max_change = omega_max*dt; 34 | 35 | if norm(xR_true(1:2,ell,k),2)>0.98*r_max 36 | %then turn back to the center: 37 | %the direction towards the center: 38 | phi_c=atan2(-xR_true(2,ell,k),-xR_true(1,ell,k)); 39 | 40 | %we turn as much as the robot can towards the center: 41 | phi1=atan2(sin(xR_true(3,ell,k-1)),cos(xR_true(3,ell,k-1))); 42 | 43 | if abs(phi1-phi_c)0.98*r_max) && abs(phi_c)>pi/10) 50 | xR_true(3,ell,k)=phi_c; 51 | end 52 | end 53 | 54 | omega(ell,k-1) = pi_to_pi(xR_true(3,ell,k)-xR_true(3,ell,k-1))/dt; 55 | end 56 | end 57 | 58 | end 59 | end %k 60 | 61 | %noisy odom measurements 62 | nv = sigma_v*randn(size(v(ell,:))); 63 | v_m(ell,:) = v(ell,:) + nv; 64 | nw = sigma_w*randn(size(omega(ell,:))); 65 | omega_m(ell,:) = omega(ell,:) + nw; 66 | 67 | end%ell 68 | 69 | 70 | 71 | %% target kinematic model: driven by continuous-time white noise 72 | 73 | %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 74 | % % **constant velocity model / zero acceleration model** 75 | % % see Bar-Shalom pp.269 76 | if dim_target ==4 77 | Q = zeros(2,2,nT,nSteps); %uncertainty of acceleration 78 | for i = 1:nT 79 | for k = 1:nSteps 80 | Q(:,:,i,k) = sigma_a^2*eye(2); % Q can also used for time-varing case 81 | end 82 | end 83 | 84 | Qd = zeros(4,4, nT,nSteps); % The Discrete State Propogate Uncertainty 85 | for i=1:nT 86 | for k = 1:nSteps 87 | Qd(:,:,i,k) = kron( [ 1/3*(dt^3) , 1/2*(dt^2); 1/2*(dt^2) , (dt) ] , Q(:,:,i,k) ); %\int [t;1]'*Q*[t;1] dt 88 | end 89 | end 90 | 91 | % Generate the Real State of Target 92 | xT_true = zeros(4,nT,nSteps); 93 | for i = 1:nT 94 | ax = zeros(1,nSteps-1); 95 | ay = zeros(1,nSteps-1); 96 | for k = 1:nSteps-1 97 | a = sqrtm(Q(:,:,k))*randn(2,1) ; 98 | ax(k) = a(1); 99 | ay(k) = a(2); 100 | end 101 | 102 | PT_init = 1e0*eye(4); %uncertainty of target's initial state 103 | xT_init_true = [10;-10;-vt;vt]; % initial state of targets 104 | xT_true(:,i,1) = xT_init_true * (-1)^0; % mvnrnd(xT_init_true, PT_init)'; 105 | for k=2:nSteps 106 | xT_true(1,i,k) = xT_true(1,i,k-1)+xT_true(3,i,k-1)*(dt) +1/2*ax(k-1)*(dt^2); 107 | xT_true(2,i,k) = xT_true(2,i,k-1)+xT_true(4,i,k-1)*(dt) +1/2*ay(k-1)*(dt^2); 108 | xT_true(3,i,k) = xT_true(3,i,k-1)+ax(k-1)*(dt); 109 | xT_true(4,i,k) = xT_true(4,i,k-1)+ay(k-1)*(dt); 110 | end 111 | end 112 | 113 | % The State Transition Matrix 114 | PHI = zeros(4,4,nT,nSteps); 115 | FF = [1 dt; 0 1]; 116 | perPhi = kron(FF, eye(2)); 117 | for i=1:nT 118 | for k=1:nSteps 119 | PHI(:,:,i,k) = perPhi; 120 | end 121 | end 122 | 123 | 124 | %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 125 | % % **constant acceleration model** 126 | % % see Bar-Shalom pp.271 127 | elseif dim_target==6 128 | Qk = sigma_j^2*eye(2); % Q can also used for time-varing case 129 | QQ = [ 1/20*dt^5, 1/8*dt^4, 1/6*dt^3 ; 130 | 1/8*dt^4, 1/3*dt^3, 1/2*dt^2 ; 131 | 1/6*dt^3, 1/2*dt^2, dt ]; 132 | Qd = zeros(6,6, nT,nSteps); % The Discrete State Propogate Uncertainty 133 | for i=1:nT 134 | for k = 1:nSteps 135 | Qd(:,:,i,k) = kron(QQ, Qk); 136 | end 137 | end 138 | 139 | % Generate the Real State of Target 140 | xT_true = zeros(6,nT,nSteps); 141 | for i = 1:nT 142 | jx = zeros(1,nSteps-1); 143 | jy = zeros(1,nSteps-1); 144 | for k = 1:nSteps-1 145 | jerk = sqrtm(Qk)*randn(2,1) ; 146 | jx(k) = jerk(1); 147 | jy(k) = jerk(2); 148 | end 149 | 150 | PT_init = 1e0*eye(6); %uncertainty of target's initial state 151 | xT_init_true = [10;-10;-vt;vt; -at; at]; % initial state of targets 152 | xT_true(:,i,1) = xT_init_true * (-1)^0; % mvnrnd(xT_init_true, PT_init)'; 153 | for k=2:nSteps 154 | xT_true(1,i,k) = xT_true(1,i,k-1)+xT_true(3,i,k-1)*(dt) +1/2*xT_true(5,i,k-1)*(dt^2) +1/6*jx(k-1)*dt^3; 155 | xT_true(2,i,k) = xT_true(2,i,k-1)+xT_true(4,i,k-1)*(dt) +1/2*xT_true(6,i,k-1)*(dt^2) +1/6*jy(k-1)*dt^3; 156 | xT_true(3,i,k) = xT_true(3,i,k-1)+xT_true(5,i,k-1)*(dt) +1/2*jx(k-1)*dt^2; 157 | xT_true(4,i,k) = xT_true(4,i,k-1)+xT_true(5,i,k-1)*(dt) +1/2*jy(k-1)*dt^2; 158 | xT_true(5,i,k) = xT_true(5,i,k-1)+jx(k-1)*dt; 159 | xT_true(6,i,k) = xT_true(6,i,k-1)+jy(k-1)*dt; 160 | end 161 | end 162 | 163 | % The State Transition Matrix 164 | FF = [1 dt 1/2*dt^2 ; 0 1 dt; 0 0 1]; 165 | perPhi = kron(FF, eye(2)); 166 | PHI = zeros(6,6,nT,nSteps); 167 | for i=1:nT 168 | for k=1:nSteps 169 | PHI(:,:,i,k) = perPhi; 170 | end 171 | end 172 | end 173 | 174 | 175 | 176 | %% generate measurements 177 | % %Note that only first curr_meas_num nonzeros are actual measuremetns 178 | zr = zeros(nR,3, nR, nSteps); %robot-to-robot 179 | zl = zeros(nR,3, nL, nSteps); %robot-to-landmark 180 | zt = zeros(nR,3, nT, nSteps); %robot-to-target 181 | 182 | for ell = 1:nR 183 | for k = 1:nSteps 184 | Rr{ell,k} = []; 185 | Rl{ell,k} = []; 186 | Rt{ell,k} = []; 187 | end 188 | end 189 | 190 | 191 | for ell = 1:nR 192 | for k = 1:nSteps 193 | 194 | 195 | %% robot-to-robot measurements 196 | 197 | curr_meas_num = 0; 198 | 199 | for j = 1:nR 200 | 201 | if j==ell, continue, end 202 | 203 | % measurement wrt *global* frame 204 | [th,r] = cart2pol(xR_true(1,j,k)-xR_true(1,ell,k), xR_true(2,j,k)-xR_true(2,ell,k)); 205 | %measurement wrt robot 206 | th = pi_to_pi(th-xR_true(3,ell,k)); 207 | 208 | if SIGPERCENT, sigma_r = sigma_p*r; end %sigma_p percentage of range 209 | 210 | %use measurement only if landmark is closer than max_range 211 | if rmin_range 212 | 213 | curr_meas_num = curr_meas_num+1; 214 | 215 | %distance-bearing meausement 216 | Rii = diag([sigma_r^2, sigma_th^2]); 217 | Rr{ell,k} = blkdiag(Rr{ell,k},Rii); 218 | %noise = mvnrnd([0;0],Rii); 219 | r = r + sigma_r*randn; %noise(1);% 220 | th = th + sigma_th*randn; %noise(2);% 221 | 222 | %store measurement, and landmark id 223 | zr(ell,1:2,curr_meas_num,k) = [r;th]; %[dx;dy]; 224 | zr(ell,3,curr_meas_num,k) = j; 225 | 226 | end 227 | 228 | end%j=nR 229 | 230 | 231 | 232 | 233 | %% robot-to-landmark measurements 234 | 235 | curr_meas_num = 0; 236 | 237 | for j = 1:nL 238 | 239 | % measurement wrt *global* frame 240 | [th,r] = cart2pol(xL_true(1,j)-xR_true(1,ell,k), xL_true(2,j)-xR_true(2,ell,k)); 241 | %measurement wrt robot 242 | th = pi_to_pi(th-xR_true(3,ell,k)); 243 | 244 | if SIGPERCENT, sigma_r = sigma_p*r; end %sigma_p percentage of range 245 | 246 | %use measurement only if landmark is closer than max_range 247 | if rmin_range 248 | 249 | curr_meas_num = curr_meas_num+1; 250 | 251 | %distance-bearing meausement 252 | Rii = diag([sigma_r^2,sigma_th^2]); 253 | Rl{ell,k} = blkdiag(Rl{ell,k},Rii); 254 | %noise = mvnrnd([0;0],Rii); 255 | r = r + sigma_r*randn; %noise(1);% 256 | th = th + sigma_th*randn; %noise(2);% 257 | 258 | %store measurement, and landmark id 259 | zl(ell,1:2, curr_meas_num,k) = [r;th];%[dx;dy];% 260 | zl(ell,3, curr_meas_num,k) = j; 261 | 262 | end 263 | 264 | end%nL 265 | 266 | 267 | %% robot-to-target measurements 268 | 269 | curr_meas_num = 0; 270 | 271 | for j = 1:nT 272 | 273 | % measurement wrt *global* frame 274 | [th,r] = cart2pol(xT_true(1,j,k)-xR_true(1,ell,k), xT_true(2,j,k)-xR_true(2,ell,k)); 275 | %measurement wrt robot 276 | th = pi_to_pi(th-xR_true(3,ell,k)); 277 | 278 | if SIGPERCENT, sigma_r = sigma_p*r; end %sigma_p percentage of range 279 | 280 | if 1 %rmin_range %always measures targets!!! 281 | 282 | curr_meas_num = curr_meas_num+1; 283 | 284 | %distance-bearing meausement 285 | Rii = diag([sigma_r^2,sigma_th^2]); 286 | Rt{ell,k} = blkdiag(Rt{ell,k},Rii); 287 | %noise = mvnrnd([0;0],Rii); 288 | r = r + sigma_r*randn; %noise(1);% 289 | th = th + sigma_th*randn; %noise(2);% 290 | 291 | %store measurement, and landmark id 292 | zt(ell,1:2, curr_meas_num,k) = [r;th];%[dx;dy];% 293 | zt(ell,3, curr_meas_num,k) = j; 294 | 295 | end 296 | 297 | end%nT 298 | 299 | 300 | end%nSteps 301 | 302 | end%ell=nR 303 | 304 | -------------------------------------------------------------------------------- /bmap.m: -------------------------------------------------------------------------------- 1 | function [x] = bmap(x, xprior,Pprior, nsteps, dt, nR, v_m, omega_m, sigma_v, sigma_w, nT, dim_target,PHIT, QTd, zr, Rr, zt, Rt, zl, Rl, nL) 2 | % % batch MAP for CLATT (cooperative localization and target tracking) 3 | 4 | 5 | %% parameters/constants 6 | global gRB 7 | J = [0 -1; 1 0]; 8 | epsilon = 1e-4; 9 | itermax = 20; %max number of iterations 10 | 11 | nddt = 3; 12 | ddt = dt/nddt; 13 | Q = [ sigma_v^2 0; 0 sigma_w^2 ]; %odometry noise 14 | 15 | dofR = 3*nR; 16 | dofT = dim_target*nT; 17 | dofk = dofR+dofT; 18 | 19 | % % first robot state is known, so excluded from the state, while first target state has prior! 20 | % state = [robots-targets; robots-targets;...] 21 | x0 = x(1:dofR,1); 22 | x = x(dofR+1:end,1); 23 | 24 | 25 | %% Gauss-Newton 26 | for iter = 1:itermax 27 | 28 | ndim = dofT + dofk*(nsteps-1); %first robot pose is known, hence not included in the state 29 | if gRB==3 30 | nmeas = dofT + 3*nR*(nsteps-1)+dim_target*nT*(nsteps-1) + 2*nR*nR* (nsteps-1) + 2*nR*nT*(nsteps-1); %may be converative!!! 31 | else 32 | nmeas = dofT + 3*nR*(nsteps-1)+dim_target*nT*(nsteps-1) + nR*nR* (nsteps-1) + nR*nT*(nsteps-1); %may be converative!!! 33 | end 34 | 35 | A = sparse(nmeas, ndim); %jacobian 36 | b = zeros(nmeas,1); %residual 37 | 38 | % % prior for the first target's state 39 | xTprior = xprior(3*nR+[1:dofT],1); 40 | PTprior = Pprior(3*nR+[1:dofT] , 3*nR+[1:dofT]); 41 | sqrtQk = real(sqrtm(inv(PTprior))); 42 | ak = xTprior - x(1:dofT,1); 43 | Gk = eye(dofT); 44 | 45 | indt2 = 1:dofT; 46 | indxm = 1:dofT; 47 | 48 | b(indxm,1) = sqrtQk'*ak ; 49 | A(indxm, indt2) = sqrtQk'*Gk ; 50 | 51 | % % all measurements 52 | for k = 2:nsteps %because first robot pose is known 53 | 54 | % % proprioception measurements % % 55 | % robot propagation 56 | for ell = 1:nR 57 | if k==2 %this actuall is the first robot in the state 58 | x1k = x0(3*ell-2:3*ell,1); %xprior(3*ell-2:3*ell,1); %x0 should be same as xprior 59 | P1k = zeros(3); 60 | for i=1:nddt 61 | xk = [ x1k(1) + v_m(ell,k-1)*ddt*cos(x1k(3)); 62 | x1k(2) + v_m(ell,k-1)*ddt*sin(x1k(3)); 63 | pi_to_pi(x1k(3) + omega_m(ell,k-1)*ddt) ]; 64 | F1k = [ 1 0 -v_m(ell,k-1)*ddt*sin(x1k(3)); 65 | 0 1 v_m(ell,k-1)*ddt*cos(x1k(3)); 66 | 0 0 1 ]; 67 | Gk = [ ddt*cos(x1k(3)) 0; 68 | ddt*sin(x1k(3)) 0; 69 | 0 ddt ]; 70 | P1k = F1k*P1k*F1k'+Gk*Q*Gk'; 71 | x1k = xk; 72 | end 73 | Qk = P1k; 74 | %sqrtQk = real(sqrtm(inv(Qk))); 75 | L = chol(Qk,'lower'); 76 | invQk = (L'\eye(size(L)))*(L\eye(size(L))); 77 | sqrtQk = sqrtm(invQk); 78 | 79 | indr2 = dofT + [3*ell-2:3*ell]; 80 | indxm =dofT + [3*ell-2:3*ell ]; 81 | 82 | Fk = - eye(3); 83 | ak = x(indr2,1) - xk; 84 | ak(3) = pi_to_pi(ak(3)); 85 | 86 | b(indxm,1) = sqrtQk'*ak ; 87 | A(indxm,indr2) = sqrtQk'*Fk ; 88 | 89 | elseif k>2 90 | indr1 = dofT + dofk*(k-3)+ [3*(ell)-2 : 3*(ell)] ; %note the first pose (k=1) is excluded in x, i.e., x_k actually corresponds to xinit_k+1 91 | 92 | x1k = x(indr1,1); 93 | P1k = zeros(3); 94 | for i=1:nddt 95 | xk = [ x1k(1) + v_m(ell,k-1)*ddt*cos(x1k(3)); 96 | x1k(2) + v_m(ell,k-1)*ddt*sin(x1k(3)); 97 | pi_to_pi(x1k(3) + omega_m(ell,k-1)*ddt) ]; %note the odometry actually is from time k 98 | F1k = [1 0 -v_m(ell,k-1)*ddt*sin(x1k(3)); 99 | 0 1 v_m(ell,k-1)*ddt*cos(x1k(3)); 100 | 0 0 1]; 101 | Gk = [ddt*cos(x1k(3)) 0; 102 | ddt*sin(x1k(3)) 0; 103 | 0 ddt]; 104 | P1k = F1k*P1k*F1k'+Gk*Q*Gk'; 105 | x1k = xk; 106 | end 107 | Qk = P1k; 108 | L = chol(Qk,'lower'); 109 | invQk = (L'\eye(size(L)))*(L\eye(size(L))); 110 | sqrtQk = sqrtm(invQk); 111 | 112 | indr2 = dofT + dofk*(k-2)+[3*(ell)-2 : 3*(ell)] ; 113 | indxm = dofT + dofk*(k-2)+[ 3*ell-2:3*ell ]; 114 | 115 | F1k = [ eye(2) J*(x(indr2(1:2),1)-x(indr1(1:2),1)); zeros(1,2) 1 ];%beware of sign 116 | Fk = - eye(3); 117 | ak = x(indr2,1) - xk; 118 | ak(3) = pi_to_pi(ak(3)); 119 | 120 | b(indxm,1) = sqrtQk'*ak ; 121 | A(indxm,indr1) = sqrtQk'*F1k ; 122 | A(indxm,indr2) = sqrtQk'*Fk ; 123 | end 124 | end 125 | 126 | 127 | % target propagation 128 | for ell = 1:nT 129 | indt1 = dofT + dofk*(k-3)+ 3*nR +[dim_target*(ell-1)+1:dim_target*ell]; 130 | indt2 = dofT + dofk*(k-2)+ 3*nR +[dim_target*(ell-1)+1:dim_target*ell]; 131 | 132 | F1k = PHIT(:,:,ell,k-1); 133 | Gk = - eye(dim_target); 134 | ak = x(indt2,1) - F1k*x(indt1,1); 135 | L = chol(QTd(:,:,ell,k-1),'lower'); 136 | invQk = (L'\eye(size(L)))*(L\eye(size(L))); 137 | sqrtQk = sqrtm(invQk); 138 | 139 | indxm = dofT + dofk*(k-2)+ 3*nR +[dim_target*(ell-1)+1:dim_target*ell]; 140 | 141 | b(indxm,1) = sqrtQk'*ak ; 142 | A(indxm, indt1) = sqrtQk'*F1k ; 143 | A(indxm, indt2) = sqrtQk'*Gk ; 144 | end 145 | 146 | 147 | 148 | % % exterocpetive measurements % % 149 | % robot-to-robot 150 | imeas = 0; 151 | for ell = 1:nR 152 | for i = 1:size(zr(ell,:,:,k),3) 153 | if zr(ell,3,i,k)<=0, continue, end 154 | 155 | Ri = Rr{ell,k}(2*i-1:2*i, 2*i-1:2*i); 156 | sqrtRk = sqrtm(inv(Ri)); 157 | 158 | imeas = imeas+1; 159 | indr1 = dofT + dofk*(k-2)+[3*ell-2:3*ell]; 160 | indr2 = dofT + dofk*(k-2)+[3*zr(ell,3,i,k)-2 : 3*zr(ell,3,i,k)]; 161 | 162 | C = [cos(x(indr1(3))) -sin(x(indr1(3))); sin(x(indr1(3))) cos(x(indr1(3))) ]; 163 | i_p_r = C'*(x(indr2(1:2))-x(indr1(1:2))); 164 | rho = norm(i_p_r); 165 | th = atan2(i_p_r(2),i_p_r(1)); 166 | zhat = [rho; th]; 167 | 168 | if gRB==3 169 | indxm = dofT + dofk*(nsteps-1) + 2*nR*nR*(k-2) + [ 2*imeas-1:2*imeas ] ; 170 | r = [ zr(ell,1,i,k)-zhat(1,1); pi_to_pi(zr(ell,2,i,k)-zhat(2,1)) ] ; %beware of sign 171 | Htem = [ 1/norm(i_p_r)*i_p_r'; 1/norm(i_p_r)^2*i_p_r'*J' ]; 172 | H1 = - Htem* C'*[ eye(2) J*(x(indr2(1:2)) - x(indr1(1:2))) ]; 173 | H2 = Htem* C'*[eye(2) zeros(2,1)]; 174 | elseif gRB==2 175 | indxm = dofT + dofk*(nsteps-1) + nR*nR*(k-2) + imeas ; 176 | r = pi_to_pi(zr(ell,2,i,k)-zhat(2,1)); %beware of sign 177 | Htem = [ 1/norm(i_p_r)*i_p_r'; 1/norm(i_p_r)^2*i_p_r'*J' ]; 178 | H1 = - Htem* C'*[ eye(2) J*(x(indr2(1:2)) - x(indr1(1:2))) ]; 179 | H2 = Htem* C'*[eye(2) zeros(2,1)]; 180 | H1 = H1(2,:); 181 | H2 = H2(2,:); 182 | sqrtRk = sqrtRk(2,2); 183 | elseif gRB==1 184 | indxm = dofT + dofk*(nsteps-1) + nR*nR*(k-2) + imeas ; 185 | r = zr(ell,1,i,k)-zhat(1,1); %beware of sign 186 | Htem = [ 1/norm(i_p_r)*i_p_r'; 1/norm(i_p_r)^2*i_p_r'*J' ]; 187 | H1 = - Htem* C'*[ eye(2) J*(x(indr2(1:2)) - x(indr1(1:2))) ]; 188 | H2 = Htem* C'*[eye(2) zeros(2,1)]; 189 | H1 = H1(1,:); 190 | H2 = H2(1,:); 191 | sqrtRk = sqrtRk(1,1); 192 | end 193 | 194 | A(indxm, [indr1, indr2]) = [sqrtRk'*H1 , sqrtRk'*H2] ; 195 | b(indxm,1) = sqrtRk'*r; 196 | end 197 | end 198 | 199 | 200 | % robot-to-target 201 | imeas = 0; 202 | for ell = 1:nR 203 | for i = 1:size(zt(ell,:,:,k),3) 204 | if zt(ell,3,i,k)<=0, continue, end 205 | 206 | Ri = Rt{ell,k}(2*i-1:2*i, 2*i-1:2*i); 207 | sqrtRk = sqrtm(inv(Ri)); 208 | 209 | imeas = imeas+1; 210 | indr1 = dofT + dofk*(k-2)+ [3*ell-2:3*ell]; 211 | indr2 = dofT + dofk*(k-2)+ 3*nR+ [dim_target*(zt(ell,3,i,k)-1)+1 : dim_target*zt(ell,3,i,k)]; 212 | 213 | C = [cos(x(indr1(3))) -sin(x(indr1(3))); sin(x(indr1(3))) cos(x(indr1(3))) ]; 214 | i_p_t = C'*(x(indr2(1:2))-x(indr1(1:2))); 215 | rho = norm(i_p_t); 216 | th = atan2(i_p_t(2),i_p_t(1)); 217 | zhat = [rho; th]; 218 | 219 | if gRB==3 220 | indxm = dofT + dofk*(nsteps-1) + 2*nR*nR*(nsteps-1) + 2*nR*nT*(k-2) + [2*imeas-1:2*imeas] ; 221 | r = [ zt(ell,1,i,k)-zhat(1,1); pi_to_pi(zt(ell,2,i,k)-zhat(2,1)) ] ; %beware of sign 222 | Htem = [ 1/norm(i_p_t)*i_p_t'; 1/norm(i_p_t)^2*i_p_t'*J' ]; 223 | H1 = - Htem* C'*[ eye(2) J*(x(indr2(1:2)) - x(indr1(1:2))) ]; 224 | H2 = Htem* C'*[eye(2) zeros(2,dim_target-2)]; 225 | elseif gRB==2 226 | indxm = dofT + dofk*(nsteps-1) + nR*nR*(nsteps-1) + nR*nT*(k-2) + imeas ; 227 | r = pi_to_pi(zt(ell,2,i,k)-zhat(2,1)) ; %beware of sign 228 | Htem = [ 1/norm(i_p_t)*i_p_t'; 1/norm(i_p_t)^2*i_p_t'*J' ]; 229 | H1 = - Htem* C'*[ eye(2) J*(x(indr2(1:2)) - x(indr1(1:2))) ]; 230 | H2 = Htem* C'*[eye(2) zeros(2,dim_target-2)]; 231 | H1 = H1(2,:); 232 | H2 = H2(2,:); 233 | sqrtRk = sqrtRk(2,2); 234 | elseif gRB==1 235 | indxm = dofT + dofk*(nsteps-1) + nR*nR*(nsteps-1) + nR*nT*(k-2) +imeas ; 236 | r = zt(ell,1,i,k)-zhat(1,1); %beware of sign 237 | Htem = [ 1/norm(i_p_t)*i_p_t'; 1/norm(i_p_t)^2*i_p_t'*J' ]; 238 | H1 = - Htem* C'*[ eye(2) J*(x(indr2(1:2)) - x(indr1(1:2))) ]; 239 | H2 = Htem* C'*[eye(2) zeros(2,dim_target-2)]; 240 | H1 = H1(1,:); 241 | H2 = H2(1,:); 242 | sqrtRk = sqrtRk(1,1); 243 | end 244 | 245 | A(indxm, [indr1, indr2]) = [sqrtRk'*H1 , sqrtRk'*H2] ; 246 | b(indxm,1) = sqrtRk'*r; 247 | end 248 | end 249 | 250 | % robot-to-landmark 251 | 252 | 253 | end%k 254 | 255 | 256 | % % solve linear system : Ax = b 257 | % % using sparse QR with reordering 258 | [~,n] = size(A); 259 | [V,beta,p,R,qp] = cs_qr(A) ; % fill-in reduced reordering qp 260 | d = cs_qleft(V, beta, p, b) ; 261 | Rp = R(1:n,1:n); 262 | bp = d(1:n,1); 263 | delta_x = Rp \ bp; 264 | delta_x(qp) = delta_x ; 265 | 266 | 267 | x = x + delta_x; 268 | 269 | ndx = norm(delta_x); 270 | if ndx 16 | %% ----------------------------------------------------------------------------------------------- 17 | 18 | 19 | clear all 20 | close all 21 | clc 22 | 23 | 24 | %% 25 | randomselect='repeatlast'; % 'random' or 'repeatlast'; 26 | switch randomselect 27 | case 'repeatlast' 28 | load randstates; 29 | rand('state',randstate); 30 | randn('state',randnstate); 31 | case 'random' 32 | % do nothing 33 | otherwise 34 | rand('state',randomselect); 35 | randn('state',randomselect); 36 | end 37 | % % 38 | randstate=rand('state'); 39 | randnstate=randn('state'); 40 | save randstates.mat randstate randnstate; 41 | 42 | 43 | %% simulation parameters %% 44 | dt = 1; %sample time rate 45 | v_true = .5; % robot linear velocity 46 | omega_true = 0.05; %robot angular velocity, 0 - line; otherwise - circle 47 | DORANDOM = 1; %random traj or not 48 | sigma = .02*v_true; 49 | sigma_v = sigma/sqrt(2); 50 | sigma_w = 2*sqrt(2)*sigma; 51 | 52 | % % measurement parameters 53 | global gRB 54 | gRB = 2; %1 - range-only; 2 - bearing-only 55 | 56 | SIGPERCENT = 1; %sigma_r is percentagae of distance or not 57 | sigma_p = .03; %noise is the percentagae of distance- measurement 58 | sigma_r = .1; %range measmnt noise 59 | sigma_th = 3*pi/180;%bearing measuremnt noise 60 | 61 | % % target stochastic motion model: constant velocity or constant acceleration 62 | dim_target = 6; %4 - constant velocity; 6 - constant accel. 63 | vt = .1; %target velocity 64 | sigma_a = .01; %acceleration noise in target motion 65 | at = 1e-6; %target accel. 66 | sigma_j = 2e-4; %jerk noise 67 | 68 | nR = 6; %number of robots 69 | nT = 3; %number of targets 70 | nL = 0; %number of landmarks, if nonzero, this is CSLAM-TT - Not used for this work. 71 | if nL>0, error('Currently we do not consider landmarks yet!'), end 72 | 73 | nSteps = 120; %nubmer of time steps 74 | nRuns = 1; %number of monte carlo runs 75 | 76 | max_range = inf; %if inf (no sensing range), then measure everything all the time 77 | min_range = 0; 78 | env_size = 100; 79 | if DORANDOM 80 | omega_max = .25; %.5;%used in random motion 81 | radius_max = 25;%the max radius of confined arena 82 | else 83 | omega_max = .05; %.5;%used in random motion 84 | radius_max = 50;%the max radius of confined arena 85 | end 86 | n_init_steps = 0; 87 | 88 | 89 | %% allocate memory for saving estimation results %% 90 | 91 | % % naive UIS: naive unscented incremental smoothing 92 | xRest_isam2 = zeros(3,nR,nSteps,nRuns); %estimated traj 93 | xRerr_isam2 = zeros(3,nR,nSteps,nRuns); %all err state 94 | Prr_isam2 = zeros(3,nR,nSteps,nRuns); %actually diag of Prr 95 | rmsRp_isam2 = zeros(nR,nSteps,nRuns); %rms of robot position 96 | rmsRth_isam2 = zeros(nR,nSteps,nRuns); %rms of robot orientation 97 | xTest_isam2 = zeros(dim_target,nT,nSteps,nRuns); %estimated target trajectory 98 | xTerr_isam2 = zeros(dim_target,nT,nSteps,nRuns); %all target error state 99 | PTT_isam2 = zeros(dim_target,dim_target,nT,nSteps,nRuns); 100 | rmsT_isam2 = zeros(nT,nSteps,nRuns); %rms of target position 101 | rmsTv_isam2 = zeros(nT,nSteps,nRuns); %rms of target velocity 102 | 103 | % % UIS: unscented incremental smoothing 104 | xRest_isam3 = zeros(3,nR,nSteps,nRuns); %estimated traj 105 | xRerr_isam3 = zeros(3,nR,nSteps,nRuns); %all err state 106 | Prr_isam3 = zeros(3,nR,nSteps,nRuns); %actually diag of Prr 107 | rmsRp_isam3 = zeros(nR,nSteps,nRuns); %rms of robot position 108 | rmsRth_isam3 = zeros(nR,nSteps,nRuns); %rms of robot orientation 109 | xTest_isam3 = zeros(dim_target,nT,nSteps,nRuns); %estimated target trajectory 110 | xTerr_isam3 = zeros(dim_target,nT,nSteps,nRuns); %all target error state 111 | PTT_isam3 = zeros(dim_target,dim_target,nT,nSteps,nRuns); 112 | rmsT_isam3 = zeros(nT,nSteps,nRuns); %rms of target position 113 | rmsTv_isam3 = zeros(nT,nSteps,nRuns); %rms of target velocity 114 | 115 | % % batch-MAP 116 | xRest_bmap = zeros(3,nR,nSteps,nRuns); %estimated traj 117 | xRerr_bmap = zeros(3,nR,nSteps,nRuns); %all err state 118 | Prr_bmap = zeros(3,nR,nSteps,nRuns); %actually diag of Prr 119 | rmsRp_bmap = zeros(nR,nSteps,nRuns); %rms of robot position 120 | rmsRth_bmap = zeros(nR,nSteps,nRuns); %rms of robot orientation 121 | xTest_bmap = zeros(dim_target,nT,nRuns); 122 | xTerr_bmap = zeros(dim_target,nT,nRuns); 123 | PTT_bmap = zeros(dim_target,nT,nRuns); 124 | rmsT_bmap = zeros(nT,nRuns); 125 | rmsTv_bmap = zeros(nT,nRuns); 126 | 127 | 128 | 129 | %% generate landmarks (same in each run) %% 130 | if nL==1 131 | xL_true_fixed = [0; v_true/omega_true]; %put the landmark at the center 132 | else 133 | if ~DORANDOM 134 | xL_true_fixed = gen_map(nL,v_true,omega_true,min_range, max_range, nSteps,dt); 135 | else 136 | xL_true_fixed = [rand(2,nL) - 0.5*ones(2,nL)] * env_size ; 137 | end 138 | end 139 | 140 | 141 | %% generate true odometry (noise-free) for all time steps %% 142 | for i = 1:nR 143 | vtemp(i,:) = v_true*ones(1,nSteps); 144 | if ~DORANDOM 145 | omegatemp(i,:) = omega_true*ones(1,nSteps) ; 146 | else 147 | omegatemp(i,:) = ((rand(1,nSteps) - .5)) *1; %random 148 | end 149 | end 150 | v_true = vtemp; 151 | omega_true = omegatemp; 152 | 153 | 154 | %% Monte Carlo simulations %% 155 | for kk = 1:nRuns 156 | 157 | kk 158 | 159 | %% generate real world simulation data % % 160 | xL_true(:,:,kk) = xL_true_fixed; 161 | [v_m,omega_m, v_true_all,omega_true_all, xR_true(:,:,:,kk), zr,Rr, zl,Rl, xT_true(:,:,:,kk),PHIT, QTd,zt,Rt] = rws(nR,nSteps,nL, xL_true(:,:,kk), dt,v_true,omega_true,sigma_v,sigma_w,sigma_r,sigma_th,sigma_p, nT,vt,sigma_a, at,sigma_j,dim_target, max_range,min_range, radius_max,omega_max, DORANDOM,SIGPERCENT); 162 | 163 | 164 | %% initialization % % 165 | % robot 166 | xRtrue0 = reshape( xR_true(:,:,1,kk), 3*nR,1); 167 | PR0 = eye(length(xRtrue0)) *1e-10; 168 | xR0 = xRtrue0 ; 169 | k = n_init_steps; 170 | 171 | % target 172 | xTtrue0 = reshape(xT_true(:,:,1,kk),dim_target*nT,1) ; 173 | if dim_target ==4 174 | PT0 = kron(eye(nT), blkdiag(eye(2)*1e0, eye(2)*1e-2)) ; 175 | elseif dim_target==6 176 | PT0 = kron(eye(nT), blkdiag( blkdiag(eye(2)*1e0, eye(2)*1e-2) , eye(2)*1e-6 ) ); 177 | else 178 | error('has to be constant velocity or constant acceleration!') 179 | end 180 | xT0 = mvnrnd(xTtrue0,PT0)'; 181 | 182 | %initial state: augmenting robot and target 183 | x0 = [xR0; xT0]; 184 | P0 = blkdiag(PR0, PT0); 185 | 186 | % naive UIS 187 | xe_isam2 = x0; 188 | Pe_isam2 = P0; 189 | xlin_isam2 = xe_isam2; 190 | Plin_isam2 = P0; 191 | xprior_isam2 = x0; 192 | Pprior_isam2 = P0; 193 | Rp_isam2 = chol(inv(P0),'lower'); 194 | bp_isam2 = zeros(length(xe_isam2),1); 195 | qp_isam2 = 1:length(xe_isam2); 196 | % 197 | for i = 1:nR 198 | xRest_isam2(:,i,k+1,kk) = xe_isam2(3*i-2:3*i,1); 199 | err = xR_true(:,i,k+1,kk) - xe_isam2(3*i-2:3*i,1); 200 | err(3) = pi_to_pi(err(3)); 201 | xRerr_isam2(:,i,k+1,kk) = err; 202 | Prr_isam2(:,i,k+1,kk) = diag(Pe_isam2(3*i-2:3*i,3*i-2:3*i)); 203 | rmsRp_isam2(i,k+1,kk) = err(1:2,1)'*err(1:2,1); 204 | rmsRth_isam2(i,k+1,kk) = err(3,1)'*err(3,1); 205 | end 206 | for i = 1:nT 207 | xTest_isam2(:,i,k+1,kk) = xe_isam2(3*nR+[dim_target*(i-1)+1:dim_target*i],1); 208 | PTT_isam2(:,:,i,k+1,kk) = PT0(dim_target*(i-1)+1:dim_target*i,dim_target*(i-1)+1:dim_target*i); 209 | Pk_isam2 = PT0(dim_target*(i-1)+1:dim_target*i,dim_target*(i-1)+1:dim_target*i); 210 | xk_isam2 = xe_isam2(3*nR+[dim_target*(i-1)+1:dim_target*i],1); 211 | err = xT_true(:,i,k+1,kk)-xk_isam2; 212 | xTerr_isam2(:,i,k+1,kk) = err; 213 | rmsT_isam2(i,k+1,kk) = err(1:2,1)'*err(1:2,1); 214 | rmsTv_isam2(i,k+1,kk) = err(3:4,1)'*err(3:4,1); 215 | end 216 | 217 | % UIS 218 | xe_isam3 = x0; 219 | Pe_isam3 = P0; 220 | xlin_isam3 = xe_isam3; 221 | Plin_isam3 = P0; 222 | xprior_isam3 = x0; 223 | Pprior_isam3 = P0; 224 | Rp_isam3 = chol(inv(P0),'lower'); 225 | bp_isam3 = zeros(length(xe_isam3),1); 226 | qp_isam3 = 1:length(xe_isam3); 227 | % 228 | for i = 1:nR 229 | xRest_isam3(:,i,k+1,kk) = xe_isam3(3*i-2:3*i,1); 230 | err = xR_true(:,i,k+1,kk) - xe_isam3(3*i-2:3*i,1); 231 | err(3) = pi_to_pi(err(3)); 232 | xRerr_isam3(:,i,k+1,kk) = err; 233 | Prr_isam3(:,i,k+1,kk) = diag(Pe_isam3(3*i-2:3*i,3*i-2:3*i)); 234 | rmsRp_isam3(i,k+1,kk) = err(1:2,1)'*err(1:2,1); 235 | rmsRth_isam3(i,k+1,kk) = err(3,1)'*err(3,1); 236 | end 237 | for i = 1:nT 238 | xTest_isam3(:,i,k+1,kk) = xe_isam3(3*nR+[dim_target*(i-1)+1:dim_target*i],1); 239 | PTT_isam3(:,:,i,k+1,kk) = PT0(dim_target*(i-1)+1:dim_target*i,dim_target*(i-1)+1:dim_target*i); 240 | Pk_isam3 = PT0(dim_target*(i-1)+1:dim_target*i,dim_target*(i-1)+1:dim_target*i); 241 | xk_isam3 = xe_isam3(3*nR+[dim_target*(i-1)+1:dim_target*i],1); 242 | err = xT_true(:,i,k+1,kk)-xk_isam3; 243 | xTerr_isam3(:,i,k+1,kk) = err; 244 | rmsT_isam3(i,k+1,kk) = err(1:2,1)'*err(1:2,1); 245 | rmsTv_isam3(i,k+1,kk) = err(3:4,1)'*err(3:4,1); 246 | end 247 | 248 | 249 | 250 | %% sequential estimation over time %% 251 | for k = n_init_steps+1:nSteps-1 %first n_init_steps for ekf propagation to produce nonzero init cov 252 | 253 | timestep = k+1 254 | 255 | dofk = 3*nR+dim_target*nT; 256 | 257 | %% Naive UIS %% 258 | if length(xe_isam2)~=3*nR*(k)+dim_target*nT*(k) 259 | error('Naive UIS: dimension of state is not correct!!!') 260 | end 261 | 262 | DO_EXACT_COV_REC = 0; %exact or approximate covariance recovery 263 | % *exact* recovering covariance used in unscented transformation, though this is expensive! 264 | if k+1>2 && DO_EXACT_COV_REC 265 | Pe_isam2 = (Rp_isam2\eye(size(Rp_isam2)))*(Rp_isam2'\eye(size(Rp_isam2))); %info matrix for the states without first robots' poses which are assumed to be known!!! 266 | Pe_isam2 = blkdiag(Pprior_isam2(1:3*nR,1:3*nR), Pe_isam2(qp_isam2,qp_isam2) ); 267 | else 268 | % already intialized 269 | end 270 | 271 | A_isam2 = Rp_isam2'*Rp_isam2; 272 | for i = 1:nR 273 | indxr = dofk*(k-1) + [ 3*i-2:3*i ]; 274 | xRi = xe_isam2(indxr,1); 275 | xRk1(3*i-2:3*i,1) = [ xRi(1,1) + v_m(i,k)*dt*cos(xRi(3,1)); 276 | xRi(2,1) + v_m(i,k)*dt*sin(xRi(3,1)); 277 | pi_to_pi(xRi(3,1) + omega_m(i,k)*dt) ]; 278 | 279 | PHI = [1 0 -v_m(i,k)*dt*sin(xRi(3,1)); 0 1 v_m(i,k)*dt*cos(xRi(3,1)); 0 0 1]; 280 | G = [dt*cos(xRi(3,1)) 0; dt*sin(xRi(3,1)) 0; 0 dt]; 281 | Q = [sigma_v^2 0; 0 sigma_w^2]; 282 | % *approx* covariance recovery for submatrix of Rp_isam2 w/o reordering 283 | if k+1>2 && ~DO_EXACT_COV_REC 284 | PRRk = inv(A_isam2(indxr-3*nR,indxr-3*nR) ); 285 | %Rtem = Rp_isam2(indxr-3*nR,indxr-3*nR); 286 | %PRRk = (Rtem\eye(size(Rtem))) * (Rtem'\eye(size(Rtem))); 287 | else 288 | PRRk = Pe_isam2(indxr,indxr); 289 | end 290 | PRRk1(3*i-2:3*i,3*i-2:3*i) = PHI * PRRk *PHI' + G*Q*G'; 291 | end 292 | 293 | for i = 1:nT 294 | indxt = dofk*(k-1)+3*nR+ [ dim_target*(i-1)+1:dim_target*i ]; 295 | xTi = xe_isam2(indxt,1); 296 | xTk1(dim_target*(i-1)+1:dim_target*i,1) = PHIT(:,:,i,k) * xTi; 297 | % *approx* covariance recovery for submatrix of Rp_isam2 w/o reordering 298 | if k+1>2 && ~DO_EXACT_COV_REC 299 | PTTk = inv(A_isam2(indxt-3*nR,indxt-3*nR) ); 300 | else 301 | PTTk = Pe_isam2(indxt,indxt); 302 | end 303 | PTTk1(dim_target*(i-1)+1:dim_target*i,dim_target*(i-1)+1:dim_target*i) = PHIT(:,:,i,k) *PTTk*PHIT(:,:,i,k)' + QTd(:,:,i,k); 304 | end 305 | xe_isam2 = [xe_isam2; xRk1; xTk1 ] ; %initial estimates, augmented by propagated new initails 306 | xlin_isam2 = [xlin_isam2; xRk1; xTk1 ] ; %linearization points, augmented by propagated new initails 307 | Ptem = blkdiag(PRRk1,PTTk1); 308 | Plin_isam2 = blkdiag(Plin_isam2,Ptem) ; 309 | 310 | [xe_isam2,xlin_isam2, Rp_isam2,bp_isam2, qp_isam2] = uis(xe_isam2,xlin_isam2, Plin_isam2, xprior_isam2,Pprior_isam2, Rp_isam2,bp_isam2,qp_isam2, k+1, dt, nR, v_m, omega_m, sigma_v, sigma_w, nT, dim_target,PHIT, QTd, zr, Rr, zt, Rt, zl, Rl, nL, false); 311 | 312 | % % save results 313 | for i = 1:nR 314 | xRest_isam2(:,i,k+1,kk) = xe_isam2(dofk*k+[3*i-2:3*i],1); 315 | err = xR_true(:,i,k+1,kk) - xe_isam2(dofk*k+[3*i-2:3*i],1); 316 | err(3) = pi_to_pi(err(3)); 317 | xRerr_isam2(:,i,k+1,kk) = err; 318 | rmsRp_isam2(i,k+1,kk) = err(1:2,1)'*err(1:2,1); 319 | rmsRth_isam2(i,k+1,kk) = err(3,1)'*err(3,1); 320 | end 321 | for i = 1:nT 322 | xTest_isam2(:,i,k+1,kk) = xe_isam2(dofk*k+3*nR+[dim_target*(i-1)+1:dim_target*i],1); 323 | xk_isam2 = xe_isam2(dofk*k+3*nR+[dim_target*(i-1)+1:dim_target*i],1); 324 | err = xT_true(:,i,k+1,kk)-xk_isam2; 325 | xTerr_isam2(:,i,k+1,kk) = err; 326 | rmsT_isam2(i,k+1,kk) = err(1:2,1)'*err(1:2,1); 327 | rmsTv_isam2(i,k+1,kk) = err(3:4,1)'*err(3:4,1); 328 | end 329 | 330 | 331 | %% UIS %% 332 | if length(xe_isam3)~=3*nR*(k)+dim_target*nT*(k) 333 | error('iSAM3: dimension of state is not correct!!!') 334 | end 335 | 336 | DO_EXACT_COV_REC = 0; %exact or approximate covariance recovery 337 | % *exact* recovering covariance used in unscented transformation though this is expensive 338 | if k+1>2 && DO_EXACT_COV_REC 339 | Pe_isam3 = (Rp_isam3\eye(size(Rp_isam3)))*(Rp_isam3'\eye(size(Rp_isam3))); %info matrix for the states without first robots' poses which are assumed to be known!!! 340 | Pe_isam3 = blkdiag(Pprior_isam3(1:3*nR,1:3*nR), Pe_isam3(qp_isam3,qp_isam3) ); 341 | else 342 | % already intialized 343 | end 344 | 345 | A_isam3 = Rp_isam3'*Rp_isam3; 346 | for i = 1:nR 347 | indxr = dofk*(k-1) + [ 3*i-2:3*i ]; 348 | xRi = xe_isam3(indxr,1); 349 | xRk1(3*i-2:3*i,1) = [ xRi(1,1) + v_m(i,k)*dt*cos(xRi(3,1)); 350 | xRi(2,1) + v_m(i,k)*dt*sin(xRi(3,1)); 351 | pi_to_pi(xRi(3,1) + omega_m(i,k)*dt) ]; 352 | 353 | PHI = [1 0 -v_m(i,k)*dt*sin(xRi(3,1)); 0 1 v_m(i,k)*dt*cos(xRi(3,1)); 0 0 1]; 354 | G = [dt*cos(xRi(3,1)) 0; dt*sin(xRi(3,1)) 0; 0 dt]; 355 | Q = [sigma_v^2 0; 0 sigma_w^2]; 356 | % *approx* covariance recovery for submatrix of Rp_isam3 w/o reordering 357 | if k+1>2 && ~DO_EXACT_COV_REC 358 | PRRk = inv(A_isam3(indxr-3*nR,indxr-3*nR) ); 359 | else 360 | PRRk = Pe_isam3(indxr,indxr); 361 | end 362 | PRRk1(3*i-2:3*i,3*i-2:3*i) = PHI * PRRk *PHI' + G*Q*G'; 363 | end 364 | 365 | for i = 1:nT 366 | indxt = dofk*(k-1)+3*nR+ [ dim_target*(i-1)+1:dim_target*i ]; 367 | xTi = xe_isam3(indxt,1); 368 | xTk1(dim_target*(i-1)+1:dim_target*i,1) = PHIT(:,:,i,k) * xTi; 369 | % *approx* covariance recovery for submatrix of Rp_isam3 w/o reordering 370 | if k+1>2 && ~DO_EXACT_COV_REC 371 | PTTk = inv(A_isam3(indxt-3*nR,indxt-3*nR) ); 372 | else 373 | PTTk = Pe_isam3(indxt,indxt); 374 | end 375 | PTTk1(dim_target*(i-1)+1:dim_target*i,dim_target*(i-1)+1:dim_target*i) = PHIT(:,:,i,k) *PTTk*PHIT(:,:,i,k)' + QTd(:,:,i,k); 376 | end 377 | xe_isam3 = [xe_isam3; xRk1; xTk1 ] ; %initial estimates, augmented by propagated new initails 378 | xlin_isam3 = [xlin_isam3; xRk1; xTk1 ] ; %linearization points, augmented by propagated new initails 379 | Ptem = blkdiag(PRRk1,PTTk1); 380 | Plin_isam3 = blkdiag(Plin_isam3,Ptem) ; 381 | 382 | [xe_isam3,xlin_isam3, Rp_isam3,bp_isam3, qp_isam3] = uis(xe_isam3,xlin_isam3, Plin_isam3, xprior_isam3,Pprior_isam3, Rp_isam3,bp_isam3,qp_isam3, k+1, dt, nR, v_m, omega_m, sigma_v, sigma_w, nT, dim_target,PHIT, QTd, zr, Rr, zt, Rt, zl, Rl, nL, true); 383 | 384 | % % save results 385 | for i = 1:nR 386 | xRest_isam3(:,i,k+1,kk) = xe_isam3(dofk*k+[3*i-2:3*i],1); 387 | err = xR_true(:,i,k+1,kk) - xe_isam3(dofk*k+[3*i-2:3*i],1); 388 | err(3) = pi_to_pi(err(3)); 389 | xRerr_isam3(:,i,k+1,kk) = err; 390 | rmsRp_isam3(i,k+1,kk) = err(1:2,1)'*err(1:2,1); 391 | rmsRth_isam3(i,k+1,kk) = err(3,1)'*err(3,1); 392 | end 393 | for i = 1:nT 394 | xTest_isam3(:,i,k+1,kk) = xe_isam3(dofk*k+3*nR+[dim_target*(i-1)+1:dim_target*i],1); 395 | xk_isam3 = xe_isam3(dofk*k+3*nR+[dim_target*(i-1)+1:dim_target*i],1); 396 | err = xT_true(:,i,k+1,kk)-xk_isam3; 397 | xTerr_isam3(:,i,k+1,kk) = err; 398 | rmsT_isam3(i,k+1,kk) = err(1:2,1)'*err(1:2,1); 399 | rmsTv_isam3(i,k+1,kk) = err(3:4,1)'*err(3:4,1); 400 | end 401 | 402 | 403 | end %k 404 | 405 | 406 | 407 | %% batch MAP %% 408 | disp('------batch MAP------') 409 | % generate initial guess: ideally use ground truth 410 | xinit_bmap(1:dofk,1) = x0; 411 | for k = 1:nSteps-1 412 | for i = 1:nR 413 | xinit_bmap(dofk*k+[3*i-2:3*i],1) = xR_true(:,i,k+1,kk) ; 414 | end 415 | for i = 1:nT 416 | xinit_bmap(dofk*k+3*nR+[dim_target*(i-1)+1:dim_target*i],1) = xT_true(:,i,k+1,kk) ; 417 | end 418 | end 419 | 420 | xe_bmap = bmap(xinit_bmap, x0,P0, k+1, dt, nR, v_m, omega_m, sigma_v, sigma_w, nT, dim_target,PHIT, QTd, zr, Rr, zt, Rt, zl, Rl, nL); 421 | 422 | % save batch results 423 | for k = 1:nSteps-1 424 | for i = 1:nR 425 | xRest_bmap(:,i,k+1,kk) = xe_bmap(dofk*k+[3*i-2:3*i],1); 426 | err = xR_true(:,i,k+1,kk) - xe_bmap(dofk*k+[3*i-2:3*i],1); 427 | err(3) = pi_to_pi(err(3)); 428 | xRerr_bmap(:,i,k+1,kk) = err; 429 | rmsRp_bmap(i,k+1,kk) = err(1:2,1)'*err(1:2,1); 430 | rmsRth_bmap(i,k+1,kk) = err(3,1)'*err(3,1); 431 | end 432 | for i = 1:nT 433 | xTest_bmap(:,i,k+1,kk) = xe_bmap(dofk*k+3*nR+[dim_target*(i-1)+1:dim_target*i],1); 434 | xk_bmap = xe_bmap(dofk*k+3*nR+[dim_target*(i-1)+1:dim_target*i],1); 435 | err = xT_true(:,i,k+1,kk)-xk_bmap; 436 | xTerr_bmap(:,i,k+1,kk) = err; 437 | rmsT_bmap(i,k+1,kk) = err(1:2,1)'*err(1:2,1); 438 | rmsTv_bmap(i,k+1,kk) = err(3:4,1)'*err(3:4,1); 439 | end 440 | end 441 | 442 | end%kk, end of monte carlo runs 443 | 444 | 445 | 446 | %% % average results over all runs 447 | % % naive UIS 448 | rmsRp_avg_isam2 = sqrt(sum(rmsRp_isam2,3)/nRuns); 449 | rmsRth_avg_isam2 = sqrt(sum(rmsRth_isam2,3)/nRuns); 450 | rmsT_avg_isam2 = sqrt(sum(rmsT_isam2,3)/nRuns); 451 | rmsTv_avg_isam2 = sqrt(sum(rmsTv_isam2,3)/nRuns); 452 | xRerr_avg_isam2 = sum(xRerr_isam2,4)/nRuns; 453 | 454 | % % UIS 455 | rmsRp_avg_isam3 = sqrt(sum(rmsRp_isam3,3)/nRuns); 456 | rmsRth_avg_isam3 = sqrt(sum(rmsRth_isam3,3)/nRuns); 457 | rmsT_avg_isam3 = sqrt(sum(rmsT_isam3,3)/nRuns); 458 | rmsTv_avg_isam3 = sqrt(sum(rmsTv_isam3,3)/nRuns); 459 | xRerr_avg_isam3 = sum(xRerr_isam3,4)/nRuns; 460 | 461 | % % batch MAP 462 | rmsRp_avg_bmap = sqrt(sum(rmsRp_bmap,3)/nRuns); 463 | rmsRth_avg_bmap = sqrt(sum(rmsRth_bmap,3)/nRuns); 464 | rmsT_avg_bmap = sqrt(sum(rmsT_bmap,3)/nRuns); 465 | rmsTv_avg_bmap = sqrt(sum(rmsTv_bmap,3)/nRuns); 466 | xRerr_avg_bmap = sum(xRerr_bmap,4)/nRuns; 467 | 468 | 469 | %% % plotting figures 470 | plot_all 471 | 472 | -------------------------------------------------------------------------------- /uis.m: -------------------------------------------------------------------------------- 1 | function [x, xlin, Rp,bp,qp] = uis(x,xlin, Plin, xprior,Pprior, Rp,bp,qp, curr_step, dt, nR, v_m, omega_m, sigma_v, sigma_w, nT, dim_target, PHIT, QTd, zr, Rr, zt, Rt, zl, Rl, nL, doOC) 2 | % % unscented incremental smoothing (UIS) for CLATT (cooperative localization and target tracking) 3 | 4 | %% parameters/constants 5 | global gRB 6 | J = [0 -1; 1 0]; 7 | 8 | % during batch update: do unscented transform or not? 9 | % it should be NOT, since 1) it is expensive to recover covariance 2) batch MAP is consistent (relinearize anyway) 10 | noUT = 1; 11 | 12 | epsilon = 1e-4; 13 | itermax = 10; %max number of iterations 14 | nrelin = 10; %frequency of relinearization 15 | 16 | nddt = 3; 17 | ddt = dt/nddt; 18 | Q = [ sigma_v^2 0; 0 sigma_w^2 ]; %odometry noise 19 | 20 | nsteps = curr_step; %number of total time steps 21 | 22 | dofR = 3*nR; 23 | dofT = dim_target*nT; 24 | dofk = dofR+dofT; 25 | 26 | % % first robot state is known, so excluded from the state, while first target state has prior! 27 | % state = [robots-targets; robots-targets;...] 28 | x0 = x(1:dofR,1); 29 | xlin0 = xlin(1:dofR,1); 30 | x = x(dofR+1:end,1); 31 | xlin = xlin(dofR+1:end,1); 32 | Plin = Plin(dofR+1:end, dofR+1:end); 33 | 34 | 35 | %% Gauss-Newton 36 | if mod(curr_step,nrelin) && curr_step>2 %incremental QR update 37 | 38 | % NOTE: run batch update for the first step in order to easily get Rp, bp, qp by QR (though not necessary) 39 | 40 | ndim = dofT + dofk*(nsteps-1); %first robot pose is known, hence not included in the state 41 | if gRB==3 42 | nmeas = dofT + 3*nR*(nsteps-1)+dim_target*nT*(nsteps-1) + 2*nR*nR* (nsteps-1) + 2*nR*nT*(nsteps-1); %may be converative!!! 43 | else 44 | nmeas = dofT + 3*nR*(nsteps-1)+dim_target*nT*(nsteps-1) + nR*nR* (nsteps-1) + nR*nT*(nsteps-1); %may be converative!!! 45 | end 46 | 47 | A = sparse(nmeas,ndim); %jacobian 48 | b = zeros(nmeas,1); %residual 49 | 50 | k = curr_step; %only compute jacobians for current time step 51 | 52 | % % proprioception measurements % % 53 | % robot propagation 54 | for ell = 1:nR 55 | if k==2 %this actuall is the first robot in the state 56 | x1k = xlin0(3*ell-2:3*ell,1); %known xlin0 or x0 57 | P1k = zeros(3); 58 | for i=1:nddt 59 | xk = [ x1k(1) + v_m(ell,k-1)*ddt*cos(x1k(3)); 60 | x1k(2) + v_m(ell,k-1)*ddt*sin(x1k(3)); 61 | pi_to_pi(x1k(3) + omega_m(ell,k-1)*ddt) ]; 62 | F1k = [ 1 0 -v_m(ell,k-1)*ddt*sin(x1k(3)); 63 | 0 1 v_m(ell,k-1)*ddt*cos(x1k(3)); 64 | 0 0 1 ]; 65 | Gk = [ ddt*cos(x1k(3)) 0; 66 | ddt*sin(x1k(3)) 0; 67 | 0 ddt ]; 68 | P1k = F1k*P1k*F1k'+Gk*Q*Gk'; 69 | x1k = xk; 70 | end 71 | Qk = P1k; 72 | L = chol(Qk,'lower'); 73 | invQk = (L'\eye(size(L)))*(L\eye(size(L))); 74 | sqrtQk = sqrtm(invQk); 75 | 76 | indr2 = dofT + [3*ell-2:3*ell]; 77 | indxm =dofT + [3*ell-2:3*ell ]; 78 | 79 | Fk = - eye(3); 80 | 81 | ak = xlin(indr2,1) - xk; 82 | ak(3) = pi_to_pi(ak(3)); 83 | 84 | b(indxm,1) = sqrtQk'*ak ; 85 | A(indxm,indr2) = sqrtQk'*Fk ; 86 | 87 | elseif k>2 88 | indr1 = dofT + dofk*(k-3)+ [3*(ell)-2 : 3*(ell)] ; %note the first pose (k=1) is excluded in x, i.e., x_k actually corresponds to xinit_k+1 89 | indr2 = dofT + dofk*(k-2)+[3*(ell)-2 : 3*(ell)] ; 90 | indxm = dofT + dofk*(k-2)+[ 3*ell-2:3*ell ]; 91 | 92 | [F1k,sqrtQk,xk] = prop_jacob_ut(@robot_motion_model, xlin(indr1,1), xlin([indr1, indr2 ],1), Plin(indr1,indr1),v_m(ell,k-1),omega_m(ell,k-1),Q,dt, doOC); 93 | 94 | Fk = - eye(3); 95 | 96 | ak = xlin(indr2,1) - xk; 97 | ak(3) = pi_to_pi(ak(3)); 98 | 99 | b(indxm,1) = sqrtQk'*ak ; 100 | A(indxm,indr1) = sqrtQk'*F1k ; 101 | A(indxm,indr2) = sqrtQk'*Fk ; 102 | end 103 | end 104 | 105 | 106 | % target propagation: since it is linear, unscented transform is same as the analytical jacobian 107 | for ell = 1:nT 108 | L = chol(QTd(:,:,ell,k-1),'lower'); 109 | invQk = (L'\eye(size(L)))*(L\eye(size(L))); 110 | sqrtQk = sqrtm(invQk); 111 | 112 | indt1 = dofT + dofk*(k-3)+ 3*nR +[dim_target*(ell-1)+1:dim_target*ell]; 113 | indt2 = dofT + dofk*(k-2)+ 3*nR +[dim_target*(ell-1)+1:dim_target*ell]; 114 | indxm = dofT + dofk*(k-2)+ 3*nR +[dim_target*(ell-1)+1:dim_target*ell]; 115 | 116 | F1k = PHIT(:,:,ell,k-1); 117 | Gk = - eye(dim_target); 118 | ak = xlin(indt2,1) - F1k*x(indt1,1); 119 | 120 | b(indxm,1) = sqrtQk'*ak ; 121 | A(indxm, indt1) = sqrtQk'*F1k ; 122 | A(indxm, indt2) = sqrtQk'*Gk ; 123 | end 124 | 125 | 126 | % % exterocpetive measurements % % 127 | 128 | % robot-to-robot 129 | imeas = 0; 130 | for ell = 1:nR 131 | for i = 1:size(zr(ell,:,:,k),3) 132 | if zr(ell,3,i,k)<=0, continue, end 133 | 134 | Ri = Rr{ell,k}(2*i-1:2*i, 2*i-1:2*i); 135 | sqrtRk = sqrtm(inv(Ri)); 136 | 137 | imeas = imeas+1; 138 | indr1 = dofT + dofk*(k-2)+[3*ell-2:3*ell]; 139 | indr2 = dofT + dofk*(k-2)+[3*zr(ell,3,i,k)-2 : 3*zr(ell,3,i,k)]; 140 | 141 | if gRB==3 142 | indxm = dofT + dofk*(nsteps-1) + 2*nR*nR*(k-2) + [ 2*imeas-1:2*imeas ] ; 143 | elseif gRB==2 144 | indxm = dofT + dofk*(nsteps-1) + nR*nR*(k-2) + imeas; 145 | sqrtRk = sqrtRk(2,2); 146 | elseif gRB==1 147 | indxm = dofT + dofk*(nsteps-1) + nR*nR*(k-2) + imeas; 148 | sqrtRk = sqrtRk(1,1); 149 | end 150 | 151 | [r,H1,H2] = meas_jacob_ut(@robot_measurement_model, xlin([indr1,indr2],1), xlin([indr1,indr2],1),Plin([indr1,indr2],[indr1,indr2]),zr(ell,1:2,i,k),Ri, doOC); 152 | 153 | A(indxm, [indr1, indr2]) = [sqrtRk'*H1 , sqrtRk'*H2] ; 154 | b(indxm,1) = sqrtRk'*r; 155 | end 156 | end 157 | 158 | % robot-to-target 159 | imeas = 0; 160 | for ell = 1:nR 161 | for i = 1:size(zt(ell,:,:,k),3) 162 | if zt(ell,3,i,k)<=0, continue, end 163 | 164 | Ri = Rt{ell,k}(2*i-1:2*i, 2*i-1:2*i); 165 | sqrtRk = sqrtm(inv(Ri)); 166 | 167 | imeas = imeas+1; 168 | indr1 = dofT + dofk*(k-2)+ [3*ell-2:3*ell]; 169 | indr2 = dofT + dofk*(k-2)+ 3*nR+ [dim_target*(zt(ell,3,i,k)-1)+1 : dim_target*zt(ell,3,i,k)]; 170 | 171 | if gRB==3 172 | indxm = dofT + dofk*(nsteps-1) + 2*nR*nR*(nsteps-1) + 2*nR*nT*(k-2) + [2*imeas-1:2*imeas] ; 173 | elseif gRB==2 174 | indxm = dofT + dofk*(nsteps-1) + nR*nR*(nsteps-1) + nR*nT*(k-2) + imeas ; 175 | sqrtRk = sqrtRk(2,2); 176 | elseif gRB==1 177 | indxm = dofT + dofk*(nsteps-1) + nR*nR*(nsteps-1) + nR*nT*(k-2) + imeas ; 178 | sqrtRk = sqrtRk(1,1); 179 | end 180 | 181 | [r,H1,H2] = meas_jacob_ut(@robot_measurement_model,xlin([indr1,indr2],1), xlin([indr1,indr2],1),Plin([indr1,indr2],[indr1,indr2]),zt(ell,1:2,i,k),Ri, doOC); 182 | 183 | A(indxm, [indr1, indr2]) = [sqrtRk'*H1 , sqrtRk'*H2] ; 184 | b(indxm,1) = sqrtRk'*r; 185 | end 186 | end 187 | 188 | 189 | % robot-to-landmark 190 | 191 | 192 | %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 193 | % reuse previously-computed jacobian and residual, and augment by the new ones 194 | qp = [qp, length(qp)+1:length(qp)+dofk]; 195 | Ak = A(:,qp); % reordering, qp, which was obtained from batch step 196 | A = sparse([ Rp, zeros(length(Rp),dofk) ; Ak ]); %match the dimension, as new robot and target states added 197 | b = [bp; b]; 198 | 199 | % solve linear system using QR 200 | % using sparse QR with reordering 201 | [~, n] = size(A); 202 | [V,beta,p,R] = cs_qr(A) ; 203 | d = cs_qleft(V, beta, p, b) ; 204 | Rp = R(1:n,1:n); 205 | bp = d(1:n,1); 206 | 207 | delta_x = Rp \ bp; 208 | delta_x(qp) = delta_x; %back to original ordering 209 | 210 | if ~doOC 211 | ndx2 = norm(delta_x); 212 | else 213 | ndx3 = norm(delta_x); 214 | end 215 | 216 | % % *linearization points, xlin* updated by deltax to be the new estimate 217 | x = xlin + delta_x; 218 | 219 | 220 | else %%%batch every *nrelin* steps (including first step)%%% 221 | 222 | 223 | for iter = 1:itermax 224 | 225 | ndim = dofT + dofk*(nsteps-1); %first robot pose is known, hence not included in the state 226 | if gRB==3 227 | nmeas = dofT + 3*nR*(nsteps-1)+dim_target*nT*(nsteps-1) + 2*nR*nR* (nsteps-1) + 2*nR*nT*(nsteps-1); %may be converative!!! 228 | else 229 | nmeas = dofT + 3*nR*(nsteps-1)+dim_target*nT*(nsteps-1) + nR*nR* (nsteps-1) + nR*nT*(nsteps-1); %may be converative!!! 230 | end 231 | 232 | A = sparse(nmeas, ndim); %jacobian 233 | b = zeros(nmeas,1); %residual 234 | 235 | % % prior for the first target's state 236 | xTprior = xprior(3*nR+[1:dofT],1); 237 | PTprior = Pprior(3*nR+[1:dofT] , 3*nR+[1:dofT]); 238 | sqrtQk = real(sqrtm(inv(PTprior))); 239 | ak = xTprior - x(1:dofT,1); 240 | Gk = eye(dofT); 241 | 242 | indt2 = 1:dofT; 243 | indxm = 1:dofT; 244 | 245 | b(indxm,1) = sqrtQk'*ak ; 246 | A(indxm, indt2) = sqrtQk'*Gk ; 247 | 248 | % % all measurements 249 | for k = 2:nsteps %because first robot pose is known 250 | 251 | % % proprioception measurements % % 252 | % robot propagation 253 | for ell = 1:nR 254 | if k==2 %this actuall is the first robot in the state 255 | x1k = x0(3*ell-2:3*ell,1); %xprior(3*ell-2:3*ell,1); %x0 should be same as xprior 256 | P1k = zeros(3); 257 | for i=1:nddt 258 | xk = [ x1k(1) + v_m(ell,k-1)*ddt*cos(x1k(3)); 259 | x1k(2) + v_m(ell,k-1)*ddt*sin(x1k(3)); 260 | pi_to_pi(x1k(3) + omega_m(ell,k-1)*ddt) ]; 261 | F1k = [ 1 0 -v_m(ell,k-1)*ddt*sin(x1k(3)); 262 | 0 1 v_m(ell,k-1)*ddt*cos(x1k(3)); 263 | 0 0 1 ]; 264 | Gk = [ ddt*cos(x1k(3)) 0; 265 | ddt*sin(x1k(3)) 0; 266 | 0 ddt ]; 267 | P1k = F1k*P1k*F1k'+Gk*Q*Gk'; 268 | x1k = xk; 269 | end 270 | Qk = P1k; 271 | L = chol(Qk,'lower'); 272 | invQk = (L'\eye(size(L)))*(L\eye(size(L))); 273 | sqrtQk = sqrtm(invQk); 274 | 275 | indr2 = dofT + [3*ell-2:3*ell]; 276 | indxm =dofT + [3*ell-2:3*ell ]; 277 | 278 | Fk = - eye(3); 279 | ak = x(indr2,1) - xk; 280 | ak(3) = pi_to_pi(ak(3)); 281 | 282 | b(indxm,1) = sqrtQk'*ak ; 283 | A(indxm,indr2) = sqrtQk'*Fk ; 284 | 285 | elseif k>2 286 | indr1 = dofT + dofk*(k-3)+ [3*(ell)-2 : 3*(ell)] ; %note the first pose (k=1) is excluded in x, i.e., x_k actually corresponds to xinit_k+1 287 | indr2 = dofT + dofk*(k-2)+[3*(ell)-2 : 3*(ell)] ; 288 | indxm = dofT + dofk*(k-2)+[ 3*ell-2:3*ell ]; 289 | 290 | if noUT 291 | x1k = x(indr1,1); 292 | P1k = zeros(3); 293 | for i=1:nddt 294 | xk = [ x1k(1) + v_m(ell,k-1)*ddt*cos(x1k(3)); 295 | x1k(2) + v_m(ell,k-1)*ddt*sin(x1k(3)); 296 | pi_to_pi(x1k(3) + omega_m(ell,k-1)*ddt) ]; %note the odometry actually is from time k 297 | F1k = [1 0 -v_m(ell,k-1)*ddt*sin(x1k(3)); 298 | 0 1 v_m(ell,k-1)*ddt*cos(x1k(3)); 299 | 0 0 1]; 300 | Gk = [ddt*cos(x1k(3)) 0; 301 | ddt*sin(x1k(3)) 0; 302 | 0 ddt]; 303 | P1k = F1k*P1k*F1k'+Gk*Q*Gk'; 304 | x1k = xk; 305 | end 306 | Qk = P1k; 307 | L = chol(Qk,'lower'); 308 | invQk = (L'\eye(size(L)))*(L\eye(size(L))); 309 | sqrtQk = sqrtm(invQk); 310 | 311 | F1k = [ eye(2) J*(x(indr2(1:2),1)-x(indr1(1:2),1)); zeros(1,2) 1 ];%beware of sign 312 | Fk = - eye(3); 313 | 314 | else %unscented 315 | [F1k,sqrtQk,xk] = prop_jacob_ut(@robot_motion_model, x(indr1,1), x([indr1, indr2 ],1), Plin(indr1,indr1),v_m(ell,k-1),omega_m(ell,k-1),Q,dt, doOC); 316 | Fk = - eye(3); 317 | end 318 | 319 | ak = xlin(indr2,1) - xk; 320 | ak(3) = pi_to_pi(ak(3)); 321 | 322 | b(indxm,1) = sqrtQk'*ak ; 323 | A(indxm,indr1) = sqrtQk'*F1k ; 324 | A(indxm,indr2) = sqrtQk'*Fk ; 325 | end 326 | end 327 | 328 | 329 | % target propagation 330 | for ell = 1:nT 331 | indt1 = dofT + dofk*(k-3)+ 3*nR +[dim_target*(ell-1)+1:dim_target*ell]; 332 | indt2 = dofT + dofk*(k-2)+ 3*nR +[dim_target*(ell-1)+1:dim_target*ell]; 333 | indxm = dofT + dofk*(k-2)+ 3*nR +[dim_target*(ell-1)+1:dim_target*ell]; 334 | 335 | L = chol(QTd(:,:,ell,k-1),'lower'); 336 | invQk = (L'\eye(size(L)))*(L\eye(size(L))); 337 | sqrtQk = sqrtm(invQk); 338 | 339 | F1k = PHIT(:,:,ell,k-1); 340 | Gk = - eye(dim_target); 341 | ak = x(indt2,1) - F1k*x(indt1,1); 342 | 343 | b(indxm,1) = sqrtQk'*ak ; 344 | A(indxm, indt1) = sqrtQk'*F1k ; 345 | A(indxm, indt2) = sqrtQk'*Gk ; 346 | end 347 | 348 | 349 | % % exterocpetive measurements % % 350 | 351 | % robot-to-robot 352 | imeas = 0; 353 | for ell = 1:nR 354 | for i = 1:size(zr(ell,:,:,k),3) 355 | if zr(ell,3,i,k)<=0, continue, end 356 | 357 | Ri = Rr{ell,k}(2*i-1:2*i, 2*i-1:2*i); 358 | sqrtRk = sqrtm(inv(Ri)); 359 | 360 | imeas = imeas+1; 361 | indr1 = dofT + dofk*(k-2)+[3*ell-2:3*ell]; 362 | indr2 = dofT + dofk*(k-2)+[3*zr(ell,3,i,k)-2 : 3*zr(ell,3,i,k)]; 363 | 364 | if noUT 365 | C = [cos(x(indr1(3))) -sin(x(indr1(3))); sin(x(indr1(3))) cos(x(indr1(3))) ]; 366 | i_p_r = C'*(x(indr2(1:2))-x(indr1(1:2))); 367 | rho = norm(i_p_r); 368 | th = atan2(i_p_r(2),i_p_r(1)); 369 | zhat = [rho; th]; 370 | 371 | if gRB==3 372 | indxm = dofT + dofk*(nsteps-1) + 2*nR*nR*(k-2) + [ 2*imeas-1:2*imeas ] ; 373 | r = [ zr(ell,1,i,k)-zhat(1,1); pi_to_pi(zr(ell,2,i,k)-zhat(2,1)) ] ; %beware of sign 374 | Htem = [ 1/norm(i_p_r)*i_p_r'; 1/norm(i_p_r)^2*i_p_r'*J' ]; 375 | H1 = - Htem* C'*[ eye(2) J*(x(indr2(1:2)) - x(indr1(1:2))) ]; 376 | H2 = Htem* C'*[eye(2) zeros(2,1)]; 377 | elseif gRB==2 378 | indxm = dofT + dofk*(nsteps-1) + nR*nR*(k-2) + imeas ; 379 | r = pi_to_pi(zr(ell,2,i,k)-zhat(2,1)); %beware of sign 380 | Htem = [ 1/norm(i_p_r)*i_p_r'; 1/norm(i_p_r)^2*i_p_r'*J' ]; 381 | H1 = - Htem* C'*[ eye(2) J*(x(indr2(1:2)) - x(indr1(1:2))) ]; 382 | H2 = Htem* C'*[eye(2) zeros(2,1)]; 383 | H1 = H1(2,:); 384 | H2 = H2(2,:); 385 | sqrtRk = sqrtRk(2,2); 386 | elseif gRB==1 387 | indxm = dofT + dofk*(nsteps-1) + nR*nR*(k-2) + imeas ; 388 | r = zr(ell,1,i,k)-zhat(1,1); %beware of sign 389 | Htem = [ 1/norm(i_p_r)*i_p_r'; 1/norm(i_p_r)^2*i_p_r'*J' ]; 390 | H1 = - Htem* C'*[ eye(2) J*(x(indr2(1:2)) - x(indr1(1:2))) ]; 391 | H2 = Htem* C'*[eye(2) zeros(2,1)]; 392 | H1 = H1(1,:); 393 | H2 = H2(1,:); 394 | sqrtRk = sqrtRk(1,1); 395 | end 396 | 397 | else %unscented 398 | if gRB==3 399 | indxm = dofT + dofk*(nsteps-1) + 2*nR*nR*(k-2) + [ 2*imeas-1:2*imeas ] ; 400 | elseif gRB==2 401 | indxm = dofT + dofk*(nsteps-1) + nR*nR*(k-2) + imeas; 402 | sqrtRk = sqrtRk(2,2); 403 | elseif gRB==1 404 | indxm = dofT + dofk*(nsteps-1) + nR*nR*(k-2) + imeas; 405 | sqrtRk = sqrtRk(1,1); 406 | end 407 | 408 | [r,H1,H2] = meas_jacob_ut(@robot_measurement_model, x([indr1,indr2],1), x([indr1,indr2],1),Plin([indr1,indr2],[indr1,indr2]),zr(ell,1:2,i,k),Ri, doOC); 409 | end 410 | 411 | A(indxm, [indr1, indr2]) = [sqrtRk'*H1 , sqrtRk'*H2] ; 412 | b(indxm,1) = sqrtRk'*r; 413 | 414 | end 415 | end 416 | 417 | 418 | % robot-to-target 419 | imeas = 0; 420 | for ell = 1:nR 421 | for i = 1:size(zt(ell,:,:,k),3) 422 | if zt(ell,3,i,k)<=0, continue, end 423 | 424 | Ri = Rt{ell,k}(2*i-1:2*i, 2*i-1:2*i); 425 | sqrtRk = sqrtm(inv(Ri)); 426 | 427 | imeas = imeas+1; 428 | indr1 = dofT + dofk*(k-2)+ [3*ell-2:3*ell]; 429 | indr2 = dofT + dofk*(k-2)+ 3*nR+ [dim_target*(zt(ell,3,i,k)-1)+1 : dim_target*zt(ell,3,i,k)]; 430 | 431 | if noUT 432 | C = [cos(x(indr1(3))) -sin(x(indr1(3))); sin(x(indr1(3))) cos(x(indr1(3))) ]; 433 | i_p_t = C'*(x(indr2(1:2))-x(indr1(1:2))); 434 | rho = norm(i_p_t); 435 | th = atan2(i_p_t(2),i_p_t(1)); 436 | zhat = [rho; th]; 437 | 438 | if gRB==3 439 | indxm = dofT + dofk*(nsteps-1) + 2*nR*nR*(nsteps-1) + 2*nR*nT*(k-2) + [2*imeas-1:2*imeas] ; 440 | r = [ zt(ell,1,i,k)-zhat(1,1); pi_to_pi(zt(ell,2,i,k)-zhat(2,1)) ] ; %beware of sign 441 | Htem = [ 1/norm(i_p_t)*i_p_t'; 1/norm(i_p_t)^2*i_p_t'*J' ]; 442 | H1 = - Htem* C'*[ eye(2) J*(x(indr2(1:2)) - x(indr1(1:2))) ]; 443 | H2 = Htem* C'*[eye(2) zeros(2,dim_target-2)]; 444 | elseif gRB==2 445 | indxm = dofT + dofk*(nsteps-1) + nR*nR*(nsteps-1) + nR*nT*(k-2) + imeas ; 446 | r = pi_to_pi(zt(ell,2,i,k)-zhat(2,1)) ; %beware of sign 447 | Htem = [ 1/norm(i_p_t)*i_p_t'; 1/norm(i_p_t)^2*i_p_t'*J' ]; 448 | H1 = - Htem* C'*[ eye(2) J*(x(indr2(1:2)) - x(indr1(1:2))) ]; 449 | H2 = Htem* C'*[eye(2) zeros(2,dim_target-2)]; 450 | H1 = H1(2,:); 451 | H2 = H2(2,:); 452 | sqrtRk = sqrtRk(2,2); 453 | elseif gRB==1 454 | indxm = dofT + dofk*(nsteps-1) + nR*nR*(nsteps-1) + nR*nT*(k-2) +imeas ; 455 | r = zt(ell,1,i,k)-zhat(1,1); %beware of sign 456 | Htem = [ 1/norm(i_p_t)*i_p_t'; 1/norm(i_p_t)^2*i_p_t'*J' ]; 457 | H1 = - Htem* C'*[ eye(2) J*(x(indr2(1:2)) - x(indr1(1:2))) ]; 458 | H2 = Htem* C'*[eye(2) zeros(2,dim_target-2)]; 459 | H1 = H1(1,:); 460 | H2 = H2(1,:); 461 | sqrtRk = sqrtRk(1,1); 462 | end 463 | 464 | else %unscented 465 | if gRB==3 466 | indxm = dofT + dofk*(nsteps-1) + 2*nR*nR*(nsteps-1) + 2*nR*nT*(k-2) + [2*imeas-1:2*imeas] ; 467 | elseif gRB==2 468 | indxm = dofT + dofk*(nsteps-1) + nR*nR*(nsteps-1) + nR*nT*(k-2) + imeas ; 469 | sqrtRk = sqrtRk(2,2); 470 | elseif gRB==1 471 | indxm = dofT + dofk*(nsteps-1) + nR*nR*(nsteps-1) + nR*nT*(k-2) + imeas ; 472 | sqrtRk = sqrtRk(1,1); 473 | end 474 | 475 | [r,H1,H2] = meas_jacob_ut(@robot_measurement_model,x([indr1,indr2],1), x([indr1,indr2],1),Plin([indr1,indr2],[indr1,indr2]),zt(ell,1:2,i,k),Ri, doOC); 476 | end 477 | 478 | A(indxm, [indr1, indr2]) = [sqrtRk'*H1 , sqrtRk'*H2] ; 479 | b(indxm,1) = sqrtRk'*r; 480 | 481 | end 482 | end 483 | 484 | % robot-to-landmark 485 | 486 | end%k 487 | 488 | 489 | %%%%%%%%%%%%%%%%%%%%% 490 | % % solve linear system : Ax = b 491 | % using sparse QR with reordering 492 | [~,n] = size(A); 493 | qp = 1:n; 494 | [V,beta,p,R] = cs_qr(A) ; 495 | d = cs_qleft(V, beta, p, b) ; 496 | Rp = R(1:n,1:n); 497 | bp = d(1:n,1); 498 | delta_x = Rp \ bp; 499 | delta_x(qp) = delta_x ; 500 | 501 | x = x + delta_x; 502 | xlin = x; 503 | 504 | if ~doOC 505 | ndx2 = norm(delta_x); 506 | if ndx22 %incremental QR update 33 | % NOTE: run batch update for the first step in order to easily get Rp, bp, qp by QR (though not necessary) 34 | 35 | ndim = dofT + dofk*(nsteps-1); %first robot pose is known, hence not included in the state 36 | if gRB==3 37 | nmeas = dofT + 3*nR*(nsteps-1)+dim_target*nT*(nsteps-1) + 2*nR*nR* (nsteps-1) + 2*nR*nT*(nsteps-1); %may be converative!!! 38 | else 39 | nmeas = dofT + 3*nR*(nsteps-1)+dim_target*nT*(nsteps-1) + nR*nR* (nsteps-1) + nR*nT*(nsteps-1); %may be converative!!! 40 | end 41 | 42 | A = sparse(nmeas,ndim); %jacobian 43 | b = zeros(nmeas,1); %residual 44 | 45 | k = curr_step; %only compute jacobians for current time step 46 | 47 | % % proprioception measurements % % 48 | % robot propagation 49 | for ell = 1:nR 50 | if k==2 %this actuall is the first robot in the state 51 | x1k = xlin0(3*ell-2:3*ell,1); %known xlin0 or x0 52 | P1k = zeros(3); 53 | for i=1:nddt 54 | xk = [ x1k(1) + v_m(ell,k-1)*ddt*cos(x1k(3)); 55 | x1k(2) + v_m(ell,k-1)*ddt*sin(x1k(3)); 56 | pi_to_pi(x1k(3) + omega_m(ell,k-1)*ddt) ]; 57 | F1k = [ 1 0 -v_m(ell,k-1)*ddt*sin(x1k(3)); 58 | 0 1 v_m(ell,k-1)*ddt*cos(x1k(3)); 59 | 0 0 1 ]; 60 | Gk = [ ddt*cos(x1k(3)) 0; 61 | ddt*sin(x1k(3)) 0; 62 | 0 ddt ]; 63 | P1k = F1k*P1k*F1k'+Gk*Q*Gk'; 64 | x1k = xk; 65 | end 66 | Qk = P1k; 67 | L = chol(Qk,'lower'); 68 | invQk = (L'\eye(size(L)))*(L\eye(size(L))); 69 | sqrtQk = sqrtm(invQk); 70 | 71 | indr2 = dofT + [3*ell-2:3*ell]; 72 | indxm =dofT + [3*ell-2:3*ell ]; 73 | 74 | Fk = - eye(3); 75 | 76 | ak = xlin(indr2,1) - xk; 77 | ak(3) = pi_to_pi(ak(3)); 78 | 79 | b(indxm,1) = sqrtQk'*ak ; 80 | A(indxm,indr2) = sqrtQk'*Fk ; 81 | 82 | elseif k>2 83 | indr1 = dofT + dofk*(k-3)+ [3*(ell)-2 : 3*(ell)] ; %note the first pose (k=1) is excluded in x, i.e., x_k actually corresponds to xinit_k+1 84 | indr2 = dofT + dofk*(k-2)+[3*(ell)-2 : 3*(ell)] ; 85 | indxm = dofT + dofk*(k-2)+[ 3*ell-2:3*ell ]; 86 | 87 | x1k = xlin(indr1,1); 88 | P1k = zeros(3); 89 | for i=1:nddt 90 | xk = [ x1k(1) + v_m(ell,k-1)*ddt*cos(x1k(3)); 91 | x1k(2) + v_m(ell,k-1)*ddt*sin(x1k(3)); 92 | pi_to_pi(x1k(3) + omega_m(ell,k-1)*ddt) ]; %note the odometry actually is from time k 93 | F1k = [1 0 -v_m(ell,k-1)*ddt*sin(x1k(3)); 94 | 0 1 v_m(ell,k-1)*ddt*cos(x1k(3)); 95 | 0 0 1]; 96 | Gk = [ddt*cos(x1k(3)) 0; 97 | ddt*sin(x1k(3)) 0; 98 | 0 ddt]; 99 | P1k = F1k*P1k*F1k'+Gk*Q*Gk'; 100 | x1k = xk; 101 | end 102 | Qk = P1k; 103 | L = chol(Qk,'lower'); 104 | invQk = (L'\eye(size(L)))*(L\eye(size(L))); 105 | sqrtQk = sqrtm(invQk); 106 | 107 | F1k = [ eye(2) J*(xlin(indr2(1:2),1)-xlin(indr1(1:2),1)); zeros(1,2) 1 ]; %beware of sign 108 | Fk = - eye(3); 109 | 110 | ak = xlin(indr2,1) - xk; 111 | ak(3) = pi_to_pi(ak(3)); 112 | 113 | b(indxm,1) = sqrtQk'*ak ; 114 | A(indxm,indr1) = sqrtQk'*F1k ; 115 | A(indxm,indr2) = sqrtQk'*Fk ; 116 | end 117 | end 118 | 119 | 120 | % target propagation 121 | for ell = 1:nT 122 | L = chol(QTd(:,:,ell,k-1),'lower'); 123 | invQk = (L'\eye(size(L)))*(L\eye(size(L))); 124 | sqrtQk = sqrtm(invQk); 125 | 126 | indt1 = dofT + dofk*(k-3)+ 3*nR +[dim_target*(ell-1)+1:dim_target*ell]; 127 | indt2 = dofT + dofk*(k-2)+ 3*nR +[dim_target*(ell-1)+1:dim_target*ell]; 128 | indxm = dofT + dofk*(k-2)+ 3*nR +[dim_target*(ell-1)+1:dim_target*ell]; 129 | 130 | F1k = PHIT(:,:,ell,k-1); 131 | Gk = - eye(dim_target); 132 | ak = xlin(indt2,1) - F1k*x(indt1,1); 133 | 134 | b(indxm,1) = sqrtQk'*ak ; 135 | A(indxm, indt1) = sqrtQk'*F1k ; 136 | A(indxm, indt2) = sqrtQk'*Gk ; 137 | end 138 | 139 | 140 | % % exterocpetive measurements % % 141 | % robot-to-robot 142 | imeas = 0; 143 | for ell = 1:nR 144 | for i = 1:size(zr(ell,:,:,k),3) 145 | if zr(ell,3,i,k)<=0, continue, end 146 | 147 | Ri = Rr{ell,k}(2*i-1:2*i, 2*i-1:2*i); 148 | sqrtRk = sqrtm(inv(Ri)); 149 | 150 | imeas = imeas+1; 151 | indr1 = dofT + dofk*(k-2)+[3*ell-2:3*ell]; 152 | indr2 = dofT + dofk*(k-2)+[3*zr(ell,3,i,k)-2 : 3*zr(ell,3,i,k)]; 153 | 154 | C = [cos(xlin(indr1(3))) -sin(xlin(indr1(3))); sin(xlin(indr1(3))) cos(xlin(indr1(3))) ]; 155 | i_p_r = C'*(xlin(indr2(1:2))-xlin(indr1(1:2))); 156 | rho = norm(i_p_r); 157 | th = atan2(i_p_r(2),i_p_r(1)); 158 | zhat = [rho; th]; 159 | 160 | if gRB==3 161 | indxm = dofT + dofk*(nsteps-1) + 2*nR*nR*(k-2) + [ 2*imeas-1:2*imeas ] ; 162 | r = [ zr(ell,1,i,k)-zhat(1,1); pi_to_pi(zr(ell,2,i,k)-zhat(2,1)) ] ; %beware of sign 163 | Htem = [ 1/norm(i_p_r)*i_p_r'; 1/norm(i_p_r)^2*i_p_r'*J' ]; 164 | H1 = - Htem* C'*[ eye(2) J*(xlin(indr2(1:2)) - xlin(indr1(1:2))) ]; 165 | H2 = Htem* C'*[eye(2) zeros(2,1)]; 166 | elseif gRB==2 167 | indxm = dofT + dofk*(nsteps-1) + nR*nR*(k-2) + imeas ; 168 | r = pi_to_pi(zr(ell,2,i,k)-zhat(2,1)) ; %beware of sign 169 | Htem = [ 1/norm(i_p_r)*i_p_r'; 1/norm(i_p_r)^2*i_p_r'*J' ]; 170 | H1 = - Htem* C'*[ eye(2) J*(xlin(indr2(1:2)) - xlin(indr1(1:2))) ]; 171 | H2 = Htem* C'*[eye(2) zeros(2,1)]; 172 | H1 = H1(2,:); 173 | H2 = H2(2,:); 174 | sqrtRk = sqrtRk(2,2); 175 | elseif gRB==1 176 | indxm = dofT + dofk*(nsteps-1) + nR*nR*(k-2) + imeas ; 177 | r = zr(ell,1,i,k)-zhat(1,1); %beware of sign 178 | Htem = [ 1/norm(i_p_r)*i_p_r'; 1/norm(i_p_r)^2*i_p_r'*J' ]; 179 | H1 = - Htem* C'*[ eye(2) J*(xlin(indr2(1:2)) - xlin(indr1(1:2))) ]; 180 | H2 = Htem* C'*[eye(2) zeros(2,1)]; 181 | H1 = H1(1,:); 182 | H2 = H2(1,:); 183 | sqrtRk = sqrtRk(1,1); 184 | end 185 | 186 | A(indxm, [indr1, indr2]) = [sqrtRk'*H1 , sqrtRk'*H2] ; 187 | b(indxm,1) = sqrtRk'*r; 188 | end 189 | end 190 | 191 | % robot-to-target 192 | imeas = 0; 193 | for ell = 1:nR 194 | for i = 1:size(zt(ell,:,:,k),3) 195 | if zt(ell,3,i,k)<=0, continue, end 196 | 197 | Ri = Rt{ell,k}(2*i-1:2*i, 2*i-1:2*i); 198 | sqrtRk = sqrtm(inv(Ri)); 199 | 200 | imeas = imeas+1; 201 | indr1 = dofT + dofk*(k-2)+ [3*ell-2:3*ell]; 202 | indr2 = dofT + dofk*(k-2)+ 3*nR+ [dim_target*(zt(ell,3,i,k)-1)+1 : dim_target*zt(ell,3,i,k)]; 203 | 204 | C = [cos(xlin(indr1(3))) -sin(xlin(indr1(3))); sin(xlin(indr1(3))) cos(xlin(indr1(3))) ]; 205 | i_p_t = C'*(xlin(indr2(1:2))-xlin(indr1(1:2))); 206 | rho = norm(i_p_t); 207 | th = atan2(i_p_t(2),i_p_t(1)); 208 | zhat = [rho; th]; 209 | 210 | if gRB==3 211 | indxm = dofT + dofk*(nsteps-1) + 2*nR*nR*(nsteps-1) + 2*nR*nT*(k-2) + [2*imeas-1:2*imeas] ; 212 | r = [ zt(ell,1,i,k)-zhat(1,1); pi_to_pi(zt(ell,2,i,k)-zhat(2,1)) ] ; %beware of sign 213 | Htem = [ 1/norm(i_p_t)*i_p_t'; 1/norm(i_p_t)^2*i_p_t'*J' ]; 214 | H1 = - Htem* C'*[ eye(2) J*(xlin(indr2(1:2)) - xlin(indr1(1:2))) ]; 215 | H2 = Htem* C'*[eye(2) zeros(2,dim_target-2)]; 216 | elseif gRB==2 217 | indxm = dofT + dofk*(nsteps-1) + nR*nR*(k-2) + imeas ; 218 | r = pi_to_pi(zt(ell,2,i,k)-zhat(2,1)) ; %beware of sign 219 | Htem = [ 1/norm(i_p_t)*i_p_t'; 1/norm(i_p_t)^2*i_p_t'*J' ]; 220 | H1 = - Htem* C'*[ eye(2) J*(xlin(indr2(1:2)) - xlin(indr1(1:2))) ]; 221 | H2 = Htem* C'*[eye(2) zeros(2,dim_target-2)]; 222 | H1 = H1(2,:); 223 | H2 = H2(2,:); 224 | sqrtRk = sqrtRk(2,2); 225 | elseif gRB==1 226 | indxm = dofT + dofk*(nsteps-1) + nR*nR*(k-2) + imeas ; 227 | r = zt(ell,1,i,k)-zhat(1,1); %beware of sign 228 | Htem = [ 1/norm(i_p_t)*i_p_t'; 1/norm(i_p_t)^2*i_p_t'*J' ]; 229 | H1 = - Htem* C'*[ eye(2) J*(xlin(indr2(1:2)) - xlin(indr1(1:2))) ]; 230 | H2 = Htem* C'*[eye(2) zeros(2,dim_target-2)]; 231 | H1 = H1(1,:); 232 | H2 = H2(1,:); 233 | sqrtRk = sqrtRk(1,1); 234 | end 235 | 236 | A(indxm, [indr1, indr2]) = [sqrtRk'*H1 , sqrtRk'*H2] ; 237 | b(indxm,1) = sqrtRk'*r; 238 | end 239 | end 240 | 241 | 242 | % robot-to-landmark 243 | 244 | 245 | %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 246 | % reuse previously-computed jacobian and residual, and augment by the new ones 247 | qp = [qp, length(qp)+1:length(qp)+dofk]; 248 | Ak = A(:,qp); % reordering, qp, which was obtained from batch step 249 | if 1 %ISNEWNODE 250 | A = sparse([ Rp, zeros(length(Rp),dofk) ; Ak ]); %match the dimension, as new robot and target states added 251 | else 252 | A = sparse([Rp; Ak]); 253 | end 254 | b = [bp; b]; 255 | 256 | % solve linear system using QR 257 | if 0% standard QR 258 | [Qp,Rp] = qr(A,0); 259 | bp = Qp'*b ; 260 | else % sparse QR with reordering 261 | [m, n] = size(A); 262 | [V,beta,p,R] = cs_qr(A) ; 263 | d = cs_qleft(V, beta, p, b) ; 264 | Rp = R(1:n,1:n); 265 | bp = d(1:n,1); 266 | end 267 | 268 | delta_x = Rp \ bp; 269 | delta_x(qp) = delta_x; %back to original ordering 270 | 271 | ndx1 = norm(delta_x); 272 | 273 | % % *linearization points, xlin* updated by deltax to be the new estimate!!! 274 | x = xlin + delta_x; 275 | 276 | 277 | 278 | else %%%batch every *nrelin* steps (including first step)%%% 279 | 280 | for iter = 1:itermax 281 | 282 | ndim = dofT + dofk*(nsteps-1); %first robot pose is known, hence not included in the state 283 | if gRB==3 284 | nmeas = dofT + 3*nR*(nsteps-1)+dim_target*nT*(nsteps-1) + 2*nR*nR* (nsteps-1) + 2*nR*nT*(nsteps-1); %may be converative!!! 285 | else 286 | nmeas = dofT + 3*nR*(nsteps-1)+dim_target*nT*(nsteps-1) + nR*nR* (nsteps-1) + nR*nT*(nsteps-1); %may be converative!!! 287 | end 288 | 289 | A = sparse(nmeas, ndim); %jacobian 290 | b = zeros(nmeas,1); %residual 291 | 292 | % % prior for the first target's state 293 | xTprior = xprior(3*nR+[1:dofT],1); 294 | PTprior = Pprior(3*nR+[1:dofT] , 3*nR+[1:dofT]); 295 | sqrtQk = real(sqrtm(inv(PTprior))); 296 | ak = xTprior - x(1:dofT,1); 297 | Gk = eye(dofT); 298 | 299 | indt2 = 1:dofT; 300 | indxm = 1:dofT; 301 | 302 | b(indxm,1) = sqrtQk'*ak ; 303 | A(indxm, indt2) = sqrtQk'*Gk ; 304 | 305 | % % all measurements 306 | for k = 2:nsteps %because first robot pose is known 307 | 308 | % % proprioception measurements % % 309 | % robot propagation 310 | for ell = 1:nR 311 | if k==2 %this actuall is the first robot in the state 312 | x1k = x0(3*ell-2:3*ell,1); %xprior(3*ell-2:3*ell,1); %x0 should be same as xprior 313 | P1k = zeros(3); 314 | for i=1:nddt 315 | xk = [ x1k(1) + v_m(ell,k-1)*ddt*cos(x1k(3)); 316 | x1k(2) + v_m(ell,k-1)*ddt*sin(x1k(3)); 317 | pi_to_pi(x1k(3) + omega_m(ell,k-1)*ddt) ]; 318 | F1k = [ 1 0 -v_m(ell,k-1)*ddt*sin(x1k(3)); 319 | 0 1 v_m(ell,k-1)*ddt*cos(x1k(3)); 320 | 0 0 1 ]; 321 | Gk = [ ddt*cos(x1k(3)) 0; 322 | ddt*sin(x1k(3)) 0; 323 | 0 ddt ]; 324 | P1k = F1k*P1k*F1k'+Gk*Q*Gk'; 325 | x1k = xk; 326 | end 327 | Qk = P1k; 328 | %sqrtQk = real(sqrtm(inv(Qk))); 329 | L = chol(Qk,'lower'); 330 | invQk = (L'\eye(size(L)))*(L\eye(size(L))); 331 | sqrtQk = sqrtm(invQk); 332 | 333 | indr2 = dofT + [3*ell-2:3*ell]; 334 | indxm =dofT + [3*ell-2:3*ell ]; 335 | 336 | Fk = - eye(3); 337 | ak = x(indr2,1) - xk; 338 | ak(3) = pi_to_pi(ak(3)); 339 | 340 | b(indxm,1) = sqrtQk'*ak ; 341 | A(indxm,indr2) = sqrtQk'*Fk ; 342 | 343 | elseif k>2 344 | indr1 = dofT + dofk*(k-3)+ [3*(ell)-2 : 3*(ell)] ; %note the first pose (k=1) is excluded in x, i.e., x_k actually corresponds to xinit_k+1 345 | indr2 = dofT + dofk*(k-2)+[3*(ell)-2 : 3*(ell)] ; 346 | indxm = dofT + dofk*(k-2)+[ 3*ell-2:3*ell ]; 347 | 348 | x1k = x(indr1,1); 349 | P1k = zeros(3); 350 | for i=1:nddt 351 | xk = [ x1k(1) + v_m(ell,k-1)*ddt*cos(x1k(3)); 352 | x1k(2) + v_m(ell,k-1)*ddt*sin(x1k(3)); 353 | pi_to_pi(x1k(3) + omega_m(ell,k-1)*ddt) ]; %note the odometry actually is from time k 354 | F1k = [1 0 -v_m(ell,k-1)*ddt*sin(x1k(3)); 355 | 0 1 v_m(ell,k-1)*ddt*cos(x1k(3)); 356 | 0 0 1]; 357 | Gk = [ddt*cos(x1k(3)) 0; 358 | ddt*sin(x1k(3)) 0; 359 | 0 ddt]; 360 | P1k = F1k*P1k*F1k'+Gk*Q*Gk'; 361 | x1k = xk; 362 | end 363 | Qk = P1k; 364 | L = chol(Qk,'lower'); 365 | invQk = (L'\eye(size(L)))*(L\eye(size(L))); 366 | sqrtQk = sqrtm(invQk); 367 | 368 | F1k = [ eye(2) J*(x(indr2(1:2),1)-x(indr1(1:2),1)); zeros(1,2) 1 ];%beware of sign 369 | Fk = - eye(3); 370 | ak = x(indr2,1) - xk; 371 | ak(3) = pi_to_pi(ak(3)); 372 | 373 | b(indxm,1) = sqrtQk'*ak ; 374 | A(indxm,indr1) = sqrtQk'*F1k ; 375 | A(indxm,indr2) = sqrtQk'*Fk ; 376 | end 377 | end 378 | 379 | 380 | % target propagation 381 | for ell = 1:nT 382 | indt1 = dofT + dofk*(k-3)+ 3*nR +[dim_target*(ell-1)+1:dim_target*ell]; 383 | indt2 = dofT + dofk*(k-2)+ 3*nR +[dim_target*(ell-1)+1:dim_target*ell]; 384 | indxm = dofT + dofk*(k-2)+ 3*nR +[dim_target*(ell-1)+1:dim_target*ell]; 385 | 386 | L = chol(QTd(:,:,ell,k-1),'lower'); 387 | invQk = (L'\eye(size(L)))*(L\eye(size(L))); 388 | sqrtQk = sqrtm(invQk); 389 | 390 | F1k = PHIT(:,:,ell,k-1); 391 | Gk = - eye(dim_target); 392 | ak = x(indt2,1) - F1k*x(indt1,1); 393 | 394 | b(indxm,1) = sqrtQk'*ak ; 395 | A(indxm, indt1) = sqrtQk'*F1k ; 396 | A(indxm, indt2) = sqrtQk'*Gk ; 397 | end 398 | 399 | 400 | % % exterocpetive measurements % % 401 | % robot-to-robot 402 | imeas = 0; 403 | for ell = 1:nR 404 | for i = 1:size(zr(ell,:,:,k),3) 405 | if zr(ell,3,i,k)<=0, continue, end 406 | 407 | Ri = Rr{ell,k}(2*i-1:2*i, 2*i-1:2*i); 408 | sqrtRk = sqrtm(inv(Ri)); 409 | 410 | imeas = imeas+1; 411 | indr1 = dofT + dofk*(k-2)+[3*ell-2:3*ell]; 412 | indr2 = dofT + dofk*(k-2)+[3*zr(ell,3,i,k)-2 : 3*zr(ell,3,i,k)]; 413 | 414 | C = [cos(x(indr1(3))) -sin(x(indr1(3))); sin(x(indr1(3))) cos(x(indr1(3))) ]; 415 | i_p_r = C'*(x(indr2(1:2))-x(indr1(1:2))); 416 | rho = norm(i_p_r); 417 | th = atan2(i_p_r(2),i_p_r(1)); 418 | zhat = [rho; th]; 419 | 420 | if gRB==3 421 | indxm = dofT + dofk*(nsteps-1) + 2*nR*nR*(k-2) + [ 2*imeas-1:2*imeas ] ; 422 | r = [ zr(ell,1,i,k)-zhat(1,1); pi_to_pi(zr(ell,2,i,k)-zhat(2,1)) ] ; %beware of sign 423 | Htem = [ 1/norm(i_p_r)*i_p_r'; 1/norm(i_p_r)^2*i_p_r'*J' ]; 424 | H1 = - Htem* C'*[ eye(2) J*(x(indr2(1:2)) - x(indr1(1:2))) ]; 425 | H2 = Htem* C'*[eye(2) zeros(2,1)]; 426 | elseif gRB==2 427 | indxm = dofT + dofk*(nsteps-1) + nR*nR*(k-2) + imeas ; 428 | r = pi_to_pi(zr(ell,2,i,k)-zhat(2,1)); %beware of sign 429 | Htem = [ 1/norm(i_p_r)*i_p_r'; 1/norm(i_p_r)^2*i_p_r'*J' ]; 430 | H1 = - Htem* C'*[ eye(2) J*(x(indr2(1:2)) - x(indr1(1:2))) ]; 431 | H2 = Htem* C'*[eye(2) zeros(2,1)]; 432 | H1 = H1(2,:); 433 | H2 = H2(2,:); 434 | sqrtRk = sqrtRk(2,2); 435 | elseif gRB==1 436 | indxm = dofT + dofk*(nsteps-1) + nR*nR*(k-2) + imeas ; 437 | r = zr(ell,1,i,k)-zhat(1,1); %beware of sign 438 | Htem = [ 1/norm(i_p_r)*i_p_r'; 1/norm(i_p_r)^2*i_p_r'*J' ]; 439 | H1 = - Htem* C'*[ eye(2) J*(x(indr2(1:2)) - x(indr1(1:2))) ]; 440 | H2 = Htem* C'*[eye(2) zeros(2,1)]; 441 | H1 = H1(1,:); 442 | H2 = H2(1,:); 443 | sqrtRk = sqrtRk(1,1); 444 | end 445 | 446 | A(indxm, [indr1, indr2]) = [sqrtRk'*H1 , sqrtRk'*H2] ; 447 | b(indxm,1) = sqrtRk'*r; 448 | end 449 | end 450 | 451 | 452 | % robot-to-target 453 | imeas = 0; 454 | for ell = 1:nR 455 | for i = 1:size(zt(ell,:,:,k),3) 456 | if zt(ell,3,i,k)<=0, continue, end 457 | 458 | Ri = Rt{ell,k}(2*i-1:2*i, 2*i-1:2*i); 459 | sqrtRk = sqrtm(inv(Ri)); 460 | 461 | imeas = imeas+1; 462 | indr1 = dofT + dofk*(k-2)+ [3*ell-2:3*ell]; 463 | indr2 = dofT + dofk*(k-2)+ 3*nR+ [dim_target*(zt(ell,3,i,k)-1)+1: dim_target*zt(ell,3,i,k)]; 464 | 465 | C = [cos(x(indr1(3))) -sin(x(indr1(3))); sin(x(indr1(3))) cos(x(indr1(3))) ]; 466 | i_p_t = C'*(x(indr2(1:2))-x(indr1(1:2))); 467 | rho = norm(i_p_t); 468 | th = atan2(i_p_t(2),i_p_t(1)); 469 | zhat = [rho; th]; 470 | 471 | if gRB==3 472 | indxm = dofT + dofk*(nsteps-1) + 2*nR*nR*(nsteps-1) + 2*nR*nT*(k-2) + [2*imeas-1:2*imeas] ; 473 | r = [ zt(ell,1,i,k)-zhat(1,1); pi_to_pi(zt(ell,2,i,k)-zhat(2,1)) ] ; %beware of sign 474 | Htem = [ 1/norm(i_p_t)*i_p_t'; 1/norm(i_p_t)^2*i_p_t'*J' ]; 475 | H1 = - Htem* C'*[ eye(2) J*(x(indr2(1:2)) - x(indr1(1:2))) ]; 476 | H2 = Htem* C'*[eye(2) zeros(2,dim_target-2)]; 477 | elseif gRB==2 478 | indxm = dofT + dofk*(nsteps-1) + nR*nR*(nsteps-1) + nR*nT*(k-2) + imeas ; 479 | r = pi_to_pi(zt(ell,2,i,k)-zhat(2,1)) ; %beware of sign 480 | Htem = [ 1/norm(i_p_t)*i_p_t'; 1/norm(i_p_t)^2*i_p_t'*J' ]; 481 | H1 = - Htem* C'*[ eye(2) J*(x(indr2(1:2)) - x(indr1(1:2))) ]; 482 | H2 = Htem* C'*[eye(2) zeros(2,dim_target-2)]; 483 | H1 = H1(2,:); 484 | H2 = H2(2,:); 485 | sqrtRk = sqrtRk(2,2); 486 | elseif gRB==1 487 | indxm = dofT + dofk*(nsteps-1) + nR*nR*(nsteps-1) + nR*nT*(k-2) +imeas ; 488 | r = zt(ell,1,i,k)-zhat(1,1); %beware of sign 489 | Htem = [ 1/norm(i_p_t)*i_p_t'; 1/norm(i_p_t)^2*i_p_t'*J' ]; 490 | H1 = - Htem* C'*[ eye(2) J*(x(indr2(1:2)) - x(indr1(1:2))) ]; 491 | H2 = Htem* C'*[eye(2) zeros(2,dim_target-2)]; 492 | H1 = H1(1,:); 493 | H2 = H2(1,:); 494 | sqrtRk = sqrtRk(1,1); 495 | end 496 | 497 | A(indxm, [indr1, indr2]) = [sqrtRk'*H1 , sqrtRk'*H2] ; 498 | b(indxm,1) = sqrtRk'*r; 499 | end 500 | end 501 | 502 | % robot-to-landmark 503 | 504 | end%k 505 | 506 | 507 | %%%%%%%%%%%%%%%%%%%%% 508 | % % solve linear system : Ax = b 509 | if 0 % standard QR 510 | [Qp,Rp] = qr(A,0); 511 | bp = Qp'*b ; 512 | delta_x = Rp \ bp; 513 | qp = 1:length(Rp); %no reordering 514 | else % sparse QR with reordering 515 | [~,n] = size(A); 516 | qp = 1:n; 517 | [V,beta,p,R,qp] = cs_qr(A) ; % fill-in reduced reordering qp 518 | d = cs_qleft(V, beta, p, b) ; 519 | Rp = R(1:n,1:n); 520 | bp = d(1:n,1); 521 | delta_x = Rp \ bp; 522 | delta_x(qp) = delta_x ; 523 | end 524 | 525 | %% 526 | x = x + delta_x; 527 | xlin = x; 528 | 529 | ndx1 = norm(delta_x); 530 | if ndx1