fixed noise model to represent uncorrelated noise on sensor readings

Author: friether

MIT_MatlabToolbox/trunk/matlab/Simulation/sim_quadrotor.slx

@@ -138,8 +138,7 @@
 
 %% Sensors
 %Noise on all states from simulation
-quadEDT.noiseStatesSensed_std       = [1 1 1 1 1 1 0.0165195073635001 0.0152648883285633 0.0215786550496705 0.000652733165165932 0.000721701528439517 0.000690781425279554];
-quadEDT.noiseStatesSensed_weights   = [0 0 0 0 0 0 0.05 0.05 0.05 1 1 1];
+quadEDT.noiseSensed_var       = diag([0.3 0.3 0.5 0.002 0.002 0.002 0.001 50]);
 
 %Delay (all sensor data)
 quadEDT.sensordelay                 = 1; %in samples of 200Hz;
@@ -173,9 +172,9 @@
 quadEDT.yawStep_time                = 5.5;
 quadEDT.yawStep_duration            = 2.5;
 %pitch
-quadEDT.pitchStep_amplitude         = 0.1; %0.1
+quadEDT.pitchStep_amplitude         = 0.2; %0.1
 quadEDT.pitchStep_time              = 3;
-quadEDT.pitchStep_duration          = 1;
+quadEDT.pitchStep_duration          = 1.5;
 %roll
 quadEDT.rollStep_amplitude          = 0.0; %0.1
 quadEDT.rollStep_time               = 3; 

MIT_MatlabToolbox/trunk/matlab/libs/RoboticsToolbox/mdl_quadrotor.m

@@ -8,7 +8,7 @@
 
 %% Generate required matrices for 
 %Load camera calibration
-load('calibration/camcalbration.mat');
+load('calibration/camcalibation.mat');
 
 %Compute matrices (use those in embedded code!)
 format long;
@@ -30,9 +30,22 @@
 % %pixel positions of those features
 % feature_pps = [cameraParams.ReprojectedPoints(1:50,:,1)';ones(1,50)];
 % 
-% %reconstruction assuming only yaw angle
+% %Note: reconstruction assumes zero pitch and roll angle and "far enough" features!
+% We would need to optimize tr(feature_pps*Lambda-(R|T)*Xw) with feature_pps
+% being the feature pixel positions, Lambda a diagonal matrix that contains
+% the inverse of the camera-z-coordinates z of each world feature, (R|T) a
+% rotation and translation matrix for 3D points in homogenous coordinates
+% and Xw, the world features in (X;Y;Z;1) representation. If we assume
+% no pitch and roll angles and sufficiently far world features, we may consider the z's similar, so Lambda
+% implodes to a scalar lambda and we can simplify solve
+% A_recon_tmp=lambda*(R|T)=features_pps*pinv(Xw). We know that
+% sqrt(A_rcn_tmp(1,1)^2+A_rcn_tmp(1,2)^2)=lambda so we can compute (R|T)
+% and derive X,Y,Z and yaw. Note that this optimization is highly sensitive to
+% non-zero pitch and roll angles.
+
 % A_rcn_tmp = intrMatrx_it*feature_pps*pinv(Xw);
-% A_rcn = A_rcn_tmp/sqrt(A_rcn_tmp(1,1)^2+A_rcn_tmp(1,2)^2);
+% A_rcn = A_rcn_tmp/sqrt(A_rcn_tmp(1,1)^2+A_rcn_tmp(1,2)^2); %This could
+% approach zero and cause numerical issues!
 % 
 % X=A_rcn(1,4)
 % Y=A_rcn(2,4)