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

fixing imu simulator for non-identical rotations

release/4.3a0
Luca 2014-04-17 16:14:32 -04:00
parent 322e3e08c8
commit a0a955e5a5
3 changed files with 7 additions and 12 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

1
matlab/unstable_examples/+imuSimulator/covarianceAnalysisCreateFactorGraph.m
View File

@ -85,7 +85,6 @@ for i=0:length(measurements)
noiseModel.Isotropic.Sigma(6, metadata.imu.epsBias))); noiseModel.Isotropic.Sigma(6, metadata.imu.epsBias)));
end end
if options.imuFactorType == 2 if options.imuFactorType == 2
% % Initialize preintegration % % Initialize preintegration
% imuMeasurement = gtsam.CombinedImuFactorPreintegratedMeasurements(... % imuMeasurement = gtsam.CombinedImuFactorPreintegratedMeasurements(...

12
matlab/unstable_examples/+imuSimulator/covarianceAnalysisCreateTrajectory.m
View File

@ -9,8 +9,7 @@ import gtsam.*;
values = Values; values = Values;
warning('fake angles! TODO: use constructor from roll-pitch-yaw when using real data') warning('fake angles! TODO: use constructor from roll-pitch-yaw when using real data - using identity rotation')
warning('using identity rotation')
if options.useRealData == 1 if options.useRealData == 1
%% Create a ground truth trajectory from Real data (if available) %% Create a ground truth trajectory from Real data (if available)
@ -55,10 +54,7 @@ if options.useRealData == 1
scenarioIndIMU2 = previousScenarioInd+j; scenarioIndIMU2 = previousScenarioInd+j;
IMUPose1 = imuSimulator.getPoseFromGtScenario(gtScenario,scenarioIndIMU1); IMUPose1 = imuSimulator.getPoseFromGtScenario(gtScenario,scenarioIndIMU1);
IMUPose2 = imuSimulator.getPoseFromGtScenario(gtScenario,scenarioIndIMU2); IMUPose2 = imuSimulator.getPoseFromGtScenario(gtScenario,scenarioIndIMU2);
IMURot2 = IMUPose2.rotation.matrix;
% scenarioIndIMU1
% IMUPose1.print('IMUPose1');
% IMUPose2.print('IMUPose2');
IMUdeltaPose = IMUPose1.between(IMUPose2); IMUdeltaPose = IMUPose1.between(IMUPose2);
IMUdeltaPoseVector = Pose3.Logmap(IMUdeltaPose); IMUdeltaPoseVector = Pose3.Logmap(IMUdeltaPose);
@ -72,10 +68,10 @@ if options.useRealData == 1
% deltaPij += deltaVij * deltaT + 0.5 * deltaRij.matrix() * biasHat.correctAccelerometer(measuredAcc) * deltaT*deltaT; % deltaPij += deltaVij * deltaT + 0.5 * deltaRij.matrix() * biasHat.correctAccelerometer(measuredAcc) * deltaT*deltaT;
% acc = (deltaPosition - initialVel * dT) * (2/dt^2) % acc = (deltaPosition - initialVel * dT) * (2/dt^2)
measurements(i).imu(j).accel = (IMUdeltaPositionVector - currentVel.*deltaT).*(2/(deltaT*deltaT)); measurements(i).imu(j).accel = IMURot2' * (IMUdeltaPositionVector - currentVel.*deltaT).*(2/(deltaT*deltaT));
% Update velocity % Update velocity
currentVel = currentVel + measurements(i).imu(j).accel * deltaT; currentVel = currentVel + IMURot2 * measurements(i).imu(j).accel * deltaT;
end end
end end