diff --git a/matlab/unstable_examples/+imuSimulator/covarianceAnalysisCreateFactorGraph.m b/matlab/unstable_examples/+imuSimulator/covarianceAnalysisCreateFactorGraph.m index 48afa7ed6..094dd10b2 100644 --- a/matlab/unstable_examples/+imuSimulator/covarianceAnalysisCreateFactorGraph.m +++ b/matlab/unstable_examples/+imuSimulator/covarianceAnalysisCreateFactorGraph.m @@ -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