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