minor refactoring
							parent
							
								
									bbde7b980c
								
							
						
					
					
						commit
						a2bf0c4da4
					
				|  | @ -123,6 +123,9 @@ void PreintegratedCombinedMeasurements::integrateMeasurement( | |||
|   F.block<3, 3>(6, 9) = vel_H_biasAcc; | ||||
|   F.block<6, 6>(9, 9) = I_6x6; | ||||
| 
 | ||||
|   // Update the uncertainty on the state (matrix F in [4]).
 | ||||
|   preintMeasCov_ = F * preintMeasCov_ * F.transpose(); | ||||
| 
 | ||||
|   // propagate uncertainty
 | ||||
|   // TODO(frank): use noiseModel routine so we can have arbitrary noise models.
 | ||||
|   const Matrix3& aCov = p().accelerometerCovariance; | ||||
|  | @ -138,15 +141,15 @@ void PreintegratedCombinedMeasurements::integrateMeasurement( | |||
|   Matrix3 wCov_updated = wCov + p().biasAccOmegaInt.block<3, 3>(3, 3); | ||||
| 
 | ||||
|   // BLOCK DIAGONAL TERMS
 | ||||
|   D_t_t(&G_measCov_Gt) = (pos_H_biasAcc | ||||
|       * (aCov / dt)  // aCov_updated / dt
 | ||||
|   D_t_t(&G_measCov_Gt) = (pos_H_biasAcc  //
 | ||||
|       * (aCov_updated / dt)  //
 | ||||
|       * pos_H_biasAcc.transpose()) + (dt * iCov); | ||||
|   D_v_v(&G_measCov_Gt) = vel_H_biasAcc | ||||
|       * (aCov / dt)  // aCov_updated / dt
 | ||||
|   D_v_v(&G_measCov_Gt) = vel_H_biasAcc  //
 | ||||
|       * (aCov_updated / dt)  //
 | ||||
|       * (vel_H_biasAcc.transpose()); | ||||
| 
 | ||||
|   D_R_R(&G_measCov_Gt) = theta_H_biasOmega | ||||
|       * (wCov / dt)  // wCov_updated / dt
 | ||||
|   D_R_R(&G_measCov_Gt) = theta_H_biasOmega  //
 | ||||
|       * (wCov_updated / dt)  //
 | ||||
|       * (theta_H_biasOmega.transpose()); | ||||
| 
 | ||||
|   D_a_a(&G_measCov_Gt) = dt * p().biasAccCovariance; | ||||
|  | @ -160,7 +163,7 @@ void PreintegratedCombinedMeasurements::integrateMeasurement( | |||
|   D_R_v(&G_measCov_Gt) = temp.transpose(); | ||||
|   D_t_v(&G_measCov_Gt) = pos_H_biasAcc * (aCov / dt) * vel_H_biasAcc.transpose(); | ||||
| 
 | ||||
|   preintMeasCov_ = F * preintMeasCov_ * F.transpose() + G_measCov_Gt; | ||||
|   preintMeasCov_.noalias() += G_measCov_Gt; | ||||
| } | ||||
| 
 | ||||
| //------------------------------------------------------------------------------
 | ||||
|  |  | |||
		Loading…
	
		Reference in New Issue