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

added IMU type 2 with noise

release/4.3a0
djensen3 2014-04-23 08:58:50 -04:00
parent 5abf0b01ea
commit 13d47fcee4
1 changed files with 29 additions and 19 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

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

@ -90,25 +90,35 @@ for i=0:length(measurements)
end
if options.imuFactorType == 2
% % Initialize preintegration
% imuMeasurement = gtsam.CombinedImuFactorPreintegratedMeasurements(...
% metadata.imu.zeroBias, ...
% metadata.imu.AccelerometerSigma.^2 * eye(3), ...
% metadata.imu.GyroscopeSigma.^2 * eye(3), ...
% metadata.imu.IntegrationSigma.^2 * eye(3), ...
% metadata.imu.BiasAccelerometerSigma.^2 * eye(3), ...
% metadata.imu.BiasGyroscopeSigma.^2 * eye(3), ...
% metadata.imu.BiasAccOmegaInit.^2 * eye(6));
% % Preintegrate
% imuMeasurement.integrateMeasurement(imuAccel, imuGyro, measurements.imu.deltaT(i));
% % Add Imu factor
% graph.add(CombinedImuFactor( ...
% currentPoseKey-1, currentVelKey-1, ...
% currentPoseKey, currentVelKey, ...
% currentBiasKey-1, currentBiasKey, ...
% imuMeasurement, ...
% metadata.imu.g, metadata.imu.omegaCoriolis, ...
% noiseModel.Isotropic.Sigma(15, metadata.imu.epsBias)));
% Initialize preintegration
imuMeasurement = gtsam.CombinedImuFactorPreintegratedMeasurements(...
metadata.imu.zeroBias, ...
metadata.imu.AccelerometerSigma.^2 * eye(3), ...
metadata.imu.GyroscopeSigma.^2 * eye(3), ...
metadata.imu.IntegrationSigma.^2 * eye(3), ...
metadata.imu.BiasAccelerometerSigma.^2 * eye(3), ...
metadata.imu.BiasGyroscopeSigma.^2 * eye(3), ...
metadata.imu.BiasAccOmegaInit.^2 * eye(6));
% Generate IMU measurements with noise
for j=1:length(measurements(i).imu) % all measurements to preintegrate
imuAccel = measurements(i).imu(j).accel ...
+ (measurementNoise.imu.accelNoiseVector .* randn(3,1))... % added noise
+ metadata.imu.accelConstantBiasVector; % constant bias
imuGyro = measurements(i).imu(j).gyro ...
+ (measurementNoise.imu.gyroNoiseVector .* randn(3,1))... % added noise
+ metadata.imu.gyroConstantBiasVector; % constant bias
% Preintegrate
imuMeasurement.integrateMeasurement(imuAccel, imuGyro, measurements(i).imu(j).deltaT);
end
% Add Imu factor
graph.add(CombinedImuFactor( ...
currentPoseKey-1, currentVelKey-1, ...
currentPoseKey, currentVelKey, ...
currentBiasKey-1, currentBiasKey, ...
imuMeasurement, ...
metadata.imu.g, metadata.imu.omegaCoriolis, ...
noiseModel.Isotropic.Sigma(15, metadata.imu.epsBias)));
end
end % end of IMU factor creation