From eaf298bd18ba4b05bbbc3228a36854ba3e881e0e Mon Sep 17 00:00:00 2001 From: Luca Date: Thu, 3 Apr 2014 11:34:26 -0400 Subject: [PATCH] starting covariance analysis on between factors (monte carlo runs) --- .../+imuSimulator/covarianceAnalysisBetween.m | 44 +++++++++++++++++++ 1 file changed, 44 insertions(+) create mode 100644 matlab/unstable_examples/+imuSimulator/covarianceAnalysisBetween.m diff --git a/matlab/unstable_examples/+imuSimulator/covarianceAnalysisBetween.m b/matlab/unstable_examples/+imuSimulator/covarianceAnalysisBetween.m new file mode 100644 index 000000000..667f26170 --- /dev/null +++ b/matlab/unstable_examples/+imuSimulator/covarianceAnalysisBetween.m @@ -0,0 +1,44 @@ +% Test GTSAM covariances on a graph with betweenFactors + +clc +clear all +close all + +%% Create ground truth trajectory +trajectoryLength = 100; + +% possibly create random trajectory +currentPoseKey = symbol('x', 0); +currentPose = Pose3; +gtValues = Values; +gtValues.insert(currentPoseKey, currentPose); + +for i=1:trajectoryLength + currentPoseKey = symbol('x', i); + deltaPosition = % create random vector with mean [x 0 0] + deltaRotation = % create random rotation with mean [0 0 0] + deltaPose = Pose3(deltaRotation, Point3(deltaPosition)); + % "Deduce" ground truth measurements + % deltaPose are the gt measurements - save them in some structure + currentPose = currentPose.compose(deltaPose); + gtValues.insert(currentPoseKey, currentPose); +end + +%% Create gt graph (using between with ground truth measurements) +% Compute covariances using gtGraph and gtValues (for visualization) + +% decide measurement covariance + +%% for k=1:numMonteCarloRuns +% create a new graph +% for each measurement. add noise and add to graph +% optimize +% compute covariances: +% compute NEES using (estimationError = estimatedValues - gtValues) and estimated covariances +% "estimationError = estimatedValues - gtValues" only holds in a linear case +% in a nonlinear case estimationError = LogMap ((estimatedValues.inverse) * gtValues) +% in GTSAM you should check "localCoordinates" + +%% compute statistics: ANEES, plots + +