diff --git a/src/processor_imu_UnitTester.h b/src/processor_imu_UnitTester.h index 5c6cef15bcbf67d9fe6c60106296fddba8a57067..4817802a021f5a8596bf1df1441ca898250650cb 100644 --- a/src/processor_imu_UnitTester.h +++ b/src/processor_imu_UnitTester.h @@ -97,8 +97,8 @@ namespace wolf { Delta_noisy_vect_ delta_noisy_vect_ 0: + 0, 0: + 0 1: +dPx, 2: +dPy, 3: +dPz 1: + dpx, 2: +dpy, 3: +dpz - 4: +dVx, 5: +dVy, 6: +dVz 4: + dvx, 5: +dvy, 6: +dvz - 7: +dOx, 8: +dOy, 9: +dOz 7: + dox, 8: +doy, 9: +doz + 4: +dOx, 5: +dOy, 6: +dOz 4: + dox, 5: +doy, 6: +doz + 7: +dVx, 8: +dVy, 9: +dVz 7: + dvx, 9: +dvy, +: +dvz */ Eigen::VectorXs Delta0_; //this will contain the Delta not affected by noise Eigen::VectorXs delta0_; //this will contain the delta not affected by noise @@ -263,16 +263,22 @@ inline IMU_jac_deltas ProcessorIMU_UnitTester::finite_diff_noise(const Scalar& _ jacobian_delta0 = jacobian_delta; //We compute all the jacobians wrt deltas and noisy deltas - for(int i=0; i<6; i++) //for 6 first component we just add to add noise as vector component since it is in the R^3 space - { + for(int i=0; i<3; i++) //for 3 first and 3 last components we just add to add noise as vector component since it is in the R^3 space + { + //PQV formulation delta_ = delta0; delta_(i) = delta_(i) + _delta_noise(i); //noise has been added delta_noisy_vect(i) = delta_; + + delta_ = delta0; + delta_(i+6) = delta_(i+6) + _delta_noise(i+6); //noise has been added + delta_noisy_vect(i+6) = delta_; } for(int i=0; i<3; i++) //for noise dtheta, it is in SO3, need to work on quaternions - { + { + //PQV formulation //fist we need to reset some stuff Eigen::Matrix3s dqr_tmp; Eigen::Vector3s dtheta = Eigen::Vector3s::Zero(); @@ -280,18 +286,23 @@ inline IMU_jac_deltas ProcessorIMU_UnitTester::finite_diff_noise(const Scalar& _ delta_ = delta0; remapDelta(delta_); //not sure that we need this dqr_tmp = Dq_out_.matrix(); - dtheta(i) += _delta_noise(i+6); //introduce perturbation + dtheta(i) += _delta_noise(i+3); //introduce perturbation dqr_tmp = dqr_tmp * v2R(dtheta); //Apply perturbation : R * exp(dtheta) --> using matrix Dq_out_ = v2q(R2v(dqr_tmp)); //orientation noise has been added --> get back to quaternion form - delta_noisy_vect(i+6) = delta_; + delta_noisy_vect(i+3) = delta_; } //We compute all the jacobians wrt Deltas and noisy Deltas - for(int i=0; i<6; i++) //for 6 first component we just add to add noise as vector component since it is in the R^3 space + for(int i=0; i<3; i++) //for 3 first and 3 last components we just add to add noise as vector component since it is in the R^3 space { + //PQV formulation Delta_ = _Delta0; Delta_(i) = Delta_(i) + _Delta_noise(i); //noise has been added Delta_noisy_vect(i) = Delta_; + + Delta_ = _Delta0; + Delta_(i+6) = Delta_(i+6) + _Delta_noise(i+6); //noise has been added + Delta_noisy_vect(i+6) = Delta_; } for(int i=0; i<3; i++) //for noise dtheta, it is in SO3, need to work on quaternions @@ -303,10 +314,10 @@ inline IMU_jac_deltas ProcessorIMU_UnitTester::finite_diff_noise(const Scalar& _ Delta_ = _Delta0; remapDelta(Delta_); //this time we need it dQr_tmp = Dq_out_.matrix(); - dtheta(i) += _Delta_noise(i+6); //introduce perturbation + dtheta(i) += _Delta_noise(i+3); //introduce perturbation dQr_tmp = dQr_tmp * v2R(dtheta); //Apply perturbation : R * exp(dtheta) --> using matrix Dq_out_ = v2q(R2v(dQr_tmp)); //orientation noise has been added --> get back to quaternion form - Delta_noisy_vect(i+6) = Delta_; + Delta_noisy_vect(i+3) = Delta_; } IMU_jac_deltas jac_deltas(_Delta0, delta0, Delta_noisy_vect, delta_noisy_vect, jacobian_delta_preint0, jacobian_delta0);