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

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

@ -84,8 +84,7 @@ for i=0:length(measurements)
graph.add(BetweenFactorConstantBias(currentBiasKey-1, currentBiasKey, metadata.imu.zeroBias, ... graph.add(BetweenFactorConstantBias(currentBiasKey-1, currentBiasKey, metadata.imu.zeroBias, ...
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(...

14
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,11 +54,8 @@ 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);
IMUdeltaRotVector = IMUdeltaPoseVector(1:3); IMUdeltaRotVector = IMUdeltaPoseVector(1:3);
@ -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

2
matlab/unstable_examples/+imuSimulator/getPoseFromGtScenario.m
View File

@ -19,7 +19,7 @@ dZ = gtECEF(3) - initialPositionECEF(3);
[xlt, ylt, zlt] = imuSimulator.ct2ENU(dX, dY, dZ,Org_lat, Org_lon); [xlt, ylt, zlt] = imuSimulator.ct2ENU(dX, dY, dZ,Org_lat, Org_lon);
gtPosition = [xlt, ylt, zlt]'; gtPosition = [xlt, ylt, zlt]';
gtRotation = Rot3; %Rot3.Expmap(rand(3,1)); %Rot3.ypr(gtScenario.Heading(scenarioInd), gtScenario.Pitch(scenarioInd), gtScenario.Roll(scenarioInd)); gtRotation = Rot3; % Rot3.Expmap(rand(3,1)); %Rot3.ypr(gtScenario.Heading(scenarioInd), gtScenario.Pitch(scenarioInd), gtScenario.Roll(scenarioInd));
currentPose = Pose3(gtRotation, Point3(gtPosition)); currentPose = Pose3(gtRotation, Point3(gtPosition));
end end