12345qiupeng
/
gtsam
1
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

TODO list

release/4.3a0
Luca 2014-02-13 10:33:45 -05:00
parent d126fc8f24
commit 0fabfc39c2
1 changed files with 22 additions and 9 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

31
matlab/unstable_examples/+imuSimulator/coriolisExample.m
View File

@ -37,8 +37,8 @@ if useRealisticValues == 1
omegaFixed = [0;0;0]; % constant rotation rate measurement omegaFixed = [0;0;0]; % constant rotation rate measurement
accelFixed = [0.5;-0.5;0]; % constant acceleration measurement accelFixed = [0.5;-0.5;0]; % constant acceleration measurement
g = [0;0;0]; % Gravity g = [0;0;0]; % Gravity
initialVelocity = [0;0;0]; % initial velocity initialVelocity = radiusEarth * omegaEarthSeconds [vector.. is a cross product, wee wiki link] ; %[0;0;0]; % TODO % initial velocity
initialPosition = [4509997.76107; 4509997.76107; 3189050]; % initial position initialPosition = [4509997.76107; 4509997.76107; 3189050]; % initial position (Earth radius 6371 km)
else else
omegaRotatingFrame = [0;0;pi/300]; % rotation of the moving frame wrt fixed frame omegaRotatingFrame = [0;0;pi/300]; % rotation of the moving frame wrt fixed frame
omegaFixed = [0;0;0]; % constant rotation rate measurement omegaFixed = [0;0;0]; % constant rotation rate measurement
@ -60,7 +60,7 @@ currentPoseFixedGT = Pose3(Rot3, Point3(initialPosition));
currentPoseRotatingGT = currentPoseFixedGT; % frames coincide for t=0 currentPoseRotatingGT = currentPoseFixedGT; % frames coincide for t=0
currentVelocityFixedGT = initialVelocity; currentVelocityFixedGT = initialVelocity;
% %
epsBias = 1e-15; epsBias = 1e-20;
sigma_init_x = noiseModel.Isotropic.Sigma(6, 1e-10); sigma_init_x = noiseModel.Isotropic.Sigma(6, 1e-10);
sigma_init_v = noiseModel.Isotropic.Sigma(3, 1e-10); sigma_init_v = noiseModel.Isotropic.Sigma(3, 1e-10);
sigma_init_b = noiseModel.Isotropic.Sigma(6, epsBias); sigma_init_b = noiseModel.Isotropic.Sigma(6, epsBias);
@ -70,9 +70,9 @@ zeroBias = imuBias.ConstantBias(zeros(3,1), zeros(3,1)); % bias is not of intere
IMU_metadata.AccelerometerSigma = 1e-5; IMU_metadata.AccelerometerSigma = 1e-5;
IMU_metadata.GyroscopeSigma = 1e-7; IMU_metadata.GyroscopeSigma = 1e-7;
IMU_metadata.IntegrationSigma = 1e-10; IMU_metadata.IntegrationSigma = 1e-10;
IMU_metadata.BiasAccelerometerSigma = 1e-5; IMU_metadata.BiasAccelerometerSigma = epsBias;
IMU_metadata.BiasGyroscopeSigma = 1e-7; IMU_metadata.BiasGyroscopeSigma = epsBias;
IMU_metadata.BiasAccOmegaInit = 1e-10; IMU_metadata.BiasAccOmegaInit = epsBias;
%% Initialize storage variables %% Initialize storage variables
positionsInFixedGT = zeros(3, length(times)); positionsInFixedGT = zeros(3, length(times));
@ -228,9 +228,7 @@ for i = 1:length(times)
currentBiasKey-1, currentBiasKey, ... currentBiasKey-1, currentBiasKey, ...
currentSummarizedMeasurement, g, omegaCoriolisIMU, ... currentSummarizedMeasurement, g, omegaCoriolisIMU, ...
noiseModel.Isotropic.Sigma(15, epsBias))); noiseModel.Isotropic.Sigma(15, epsBias)));
else % TODO: prior on biases?
error('imuSimulator:coriolisExample:IMU_typeNotFound', ...
'IMU_type = %d does not exist.\nAvailable IMU types are 1 and 2\n', IMU_type);
end end
% Add values to the graph. Use the current pose and velocity % Add values to the graph. Use the current pose and velocity
@ -378,3 +376,18 @@ zlabel('Z axis')
axis equal axis equal
grid on; grid on;
hold off; hold off;
% TODO: logging rotation errors
for all time steps
Rerror = Rgt' * Restimated;
% transforming rotation matrix to axis-angle representation
vector_error = Rot3.Logmap(Rerror);
norm(vector_error)
axis angle: [u,theta], with norm(u)=1
vector_error = u * theta;
% TODO: logging velocity errors
velocities..