v0.1-Combined EKF and fullslam.

v0.2-Includes smoothing constraint

v0.4-Includes Cholesky, Levenberg-Marquardt. (works apart frm when no features obsereved at a pose…)

v0.5- Working code with the constrained formulation (Odo formulation is probably not complete-> problem with the det(Jh'*Q'Jh) being zero)

v0.6 - formulated odo to be three dimensional [u, w, 0]'

This runs multiple runs of various SLAM algoritms and generate the average NEES and bounds.

Few modifications are needed in each slam code importantly where the read_odometry(…) called. Here give the write path to read the robot_odometry.dat. Also for each slam algo the resulting NEES per each trial is saved in the respective data directory und ***_nees_results.txt.

Also change fn: plot_stat_cov(…, data_path)

Currently on cluster:

• drive_xyzslam() - tested with data_path='/home/dhera/_Aus2005/synced/data/rectangle_cubicle_simulation2/lc_noise_ng_134_153_065_sigma';
  xyz_nees_results.txt
• drive_uvdslam() - tested with above data set. uvd_nees_results.txt
• drive_UT_uvdslam_v_0_6() - tested with above data set. utf_nees_results.txt. Data set reduced to eliminate several trials with badly scaled results. (trial size 36)

%—————————————————-
% damith.herath@eng.uts.edu.au
% LS Optimization - Full SLAM integrating odometry
% v0.1 - 12 Feb 2007
%
%—————————————————-

mapper3D_analyzer(path,n_of_images)
%—————————————————————
% Written by damith.herath@eng.uts.edu.au
% v0.2
%
%—————————————————————

 derivative of mapper3D, Current version uses scan_obs (from drive_uvdslam_sd) to read xxx_output.txt files and will plot individual ftr behaviour.

function drive_uvdslam_sd();
%—————————————————-
% uvd SLAM v0.6 with save observation data for full slam
% 03 Aug 2006
% initialisation error fixed
% all graphics handled in one routine
% uvd update only
% NEES, Q in prediction
% To work on real data (save_estimates)
% NOTE: this will drop the last non-obsrvd ftrs frm ftr_lst!!
%—————————————————-

function fullslam2_nf()
%—————————————————-
% LS Optimization - Full SLAM integrating odometry
% v0.5 - 05 Jan 2007 Reads data from the new format generated from
% drive_uvdslam_sd()
% derived from fullslam v0.9 - 21 Nov 2006 on the cluster
% bug fix: Remove ftrs without ekf estimates being used in observation list
% New formulation with [v,w] as observation
% bug fix: miss-aligned covariance plot
% bug fix: turn-rate(w) error normalizing angel
%—————————————————-

current version
% uvd SLAM v0.6
% 03 Aug 2006
% initialisation error fixed
% all graphics handled in one routine
% uvd update only
% NEES, Q in prediction
% To work on real data (save_estimates)

This code does the LS optimisation on both observations and odometry

% LS Optimization - Full SLAM integrating odometry
% v0.4 - 05 Jan 2007
% derived from fullslam v0.9 - 21 Nov 2006 on the cluster
% bug fix: Remove ftrs without ekf estimates being used in observation list
% New formulation with [v,w] as observation
% bug fix: miss-aligned covariance plot
% bug fix: turn-rate(w) error normalizing angel