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

Updated Imu Factors for changes in the ImuBias class

release/4.3a0
Stephen Williams 2013-05-19 20:25:49 +00:00
parent 54b094facb
commit dbc05045cd
2 changed files with 22 additions and 22 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

38
gtsam/navigation/EquivInertialNavFactor_GlobalVel.h
View File

@ -171,11 +171,11 @@ public:
POSE predictPose(const POSE& Pose1, const VELOCITY& Vel1, const IMUBIAS& Bias1) const { POSE predictPose(const POSE& Pose1, const VELOCITY& Vel1, const IMUBIAS& Bias1) const {
// Correct delta_pos_in_t0_ using (Bias1 - Bias_t0) // Correct delta_pos_in_t0_ using (Bias1 - Bias_t0)
Vector delta_BiasAcc = Bias1.bias_acc(); Vector delta_BiasAcc = Bias1.accelerometer();
Vector delta_BiasGyro = Bias1.bias_gyro(); Vector delta_BiasGyro = Bias1.gyroscope();
if (Bias_initial_){ if (Bias_initial_){
delta_BiasAcc -= Bias_initial_->bias_acc(); delta_BiasAcc -= Bias_initial_->accelerometer();
delta_BiasGyro -= Bias_initial_->bias_gyro(); delta_BiasGyro -= Bias_initial_->gyroscope();
} }
Matrix J_Pos_wrt_BiasAcc = Jacobian_wrt_t0_Overall_.block(4,9,3,3); Matrix J_Pos_wrt_BiasAcc = Jacobian_wrt_t0_Overall_.block(4,9,3,3);
@ -236,11 +236,11 @@ public:
VELOCITY predictVelocity(const POSE& Pose1, const VELOCITY& Vel1, const IMUBIAS& Bias1) const { VELOCITY predictVelocity(const POSE& Pose1, const VELOCITY& Vel1, const IMUBIAS& Bias1) const {
// Correct delta_vel_in_t0_ using (Bias1 - Bias_t0) // Correct delta_vel_in_t0_ using (Bias1 - Bias_t0)
Vector delta_BiasAcc = Bias1.bias_acc(); Vector delta_BiasAcc = Bias1.accelerometer();
Vector delta_BiasGyro = Bias1.bias_gyro(); Vector delta_BiasGyro = Bias1.gyroscope();
if (Bias_initial_){ if (Bias_initial_){
delta_BiasAcc -= Bias_initial_->bias_acc(); delta_BiasAcc -= Bias_initial_->accelerometer();
delta_BiasGyro -= Bias_initial_->bias_gyro(); delta_BiasGyro -= Bias_initial_->gyroscope();
} }
Matrix J_Vel_wrt_BiasAcc = Jacobian_wrt_t0_Overall_.block(6,9,3,3); Matrix J_Vel_wrt_BiasAcc = Jacobian_wrt_t0_Overall_.block(6,9,3,3);
@ -353,11 +353,11 @@ public:
// Correct delta_pos_in_t0_ using (Bias1 - Bias_t0) // Correct delta_pos_in_t0_ using (Bias1 - Bias_t0)
Vector delta_BiasAcc = Bias1.bias_acc(); Vector delta_BiasAcc = Bias1.accelerometer();
Vector delta_BiasGyro = Bias1.bias_gyro(); Vector delta_BiasGyro = Bias1.gyroscope();
if (Bias_initial){ if (Bias_initial){
delta_BiasAcc -= Bias_initial->bias_acc(); delta_BiasAcc -= Bias_initial->accelerometer();
delta_BiasGyro -= Bias_initial->bias_gyro(); delta_BiasGyro -= Bias_initial->gyroscope();
} }
Matrix J_Pos_wrt_BiasAcc = Jacobian_wrt_t0_Overall.block(4,9,3,3); Matrix J_Pos_wrt_BiasAcc = Jacobian_wrt_t0_Overall.block(4,9,3,3);
@ -381,11 +381,11 @@ public:
const boost::optional<IMUBIAS>& Bias_initial = boost::none) { const boost::optional<IMUBIAS>& Bias_initial = boost::none) {
// Correct delta_vel_in_t0_ using (Bias1 - Bias_t0) // Correct delta_vel_in_t0_ using (Bias1 - Bias_t0)
Vector delta_BiasAcc = Bias1.bias_acc(); Vector delta_BiasAcc = Bias1.accelerometer();
Vector delta_BiasGyro = Bias1.bias_gyro(); Vector delta_BiasGyro = Bias1.gyroscope();
if (Bias_initial){ if (Bias_initial){
delta_BiasAcc -= Bias_initial->bias_acc(); delta_BiasAcc -= Bias_initial->accelerometer();
delta_BiasGyro -= Bias_initial->bias_gyro(); delta_BiasGyro -= Bias_initial->gyroscope();
} }
Matrix J_Vel_wrt_BiasAcc = Jacobian_wrt_t0_Overall.block(6,9,3,3); Matrix J_Vel_wrt_BiasAcc = Jacobian_wrt_t0_Overall.block(6,9,3,3);
@ -481,12 +481,12 @@ public:
// Note: all delta terms refer to an IMU\sensor system at t0 // Note: all delta terms refer to an IMU\sensor system at t0
// Calculate the corrected measurements using the Bias object // Calculate the corrected measurements using the Bias object
Vector AccCorrected = Bias_t0.CorrectAcc(msr_acc_t); Vector AccCorrected = Bias_t0.correctAccelerometer(msr_acc_t);
Vector body_t_a_body; Vector body_t_a_body;
if (flag_use_body_P_sensor){ if (flag_use_body_P_sensor){
Matrix body_R_sensor = body_P_sensor.rotation().matrix(); Matrix body_R_sensor = body_P_sensor.rotation().matrix();
Vector GyroCorrected(Bias_t0.CorrectGyro(msr_gyro_t)); Vector GyroCorrected(Bias_t0.correctGyroscope(msr_gyro_t));
Vector body_omega_body = body_R_sensor * GyroCorrected; Vector body_omega_body = body_R_sensor * GyroCorrected;
Matrix body_omega_body__cross = skewSymmetric(body_omega_body); Matrix body_omega_body__cross = skewSymmetric(body_omega_body);
@ -509,7 +509,7 @@ public:
// Note: all delta terms refer to an IMU\sensor system at t0 // Note: all delta terms refer to an IMU\sensor system at t0
// Calculate the corrected measurements using the Bias object // Calculate the corrected measurements using the Bias object
Vector GyroCorrected = Bias_t0.CorrectGyro(msr_gyro_t); Vector GyroCorrected = Bias_t0.correctGyroscope(msr_gyro_t);
Vector body_t_omega_body; Vector body_t_omega_body;
if (flag_use_body_P_sensor){ if (flag_use_body_P_sensor){

6
gtsam/navigation/InertialNavFactor_GlobalVelocity.h
View File

@ -148,7 +148,7 @@ public:
POSE predictPose(const POSE& Pose1, const VELOCITY& Vel1, const IMUBIAS& Bias1) const { POSE predictPose(const POSE& Pose1, const VELOCITY& Vel1, const IMUBIAS& Bias1) const {
// Calculate the corrected measurements using the Bias object // Calculate the corrected measurements using the Bias object
Vector GyroCorrected(Bias1.CorrectGyro(measurement_gyro_)); Vector GyroCorrected(Bias1.correctGyroscope(measurement_gyro_));
const POSE& world_P1_body = Pose1; const POSE& world_P1_body = Pose1;
const VELOCITY& world_V1_body = Vel1; const VELOCITY& world_V1_body = Vel1;
@ -175,7 +175,7 @@ public:
VELOCITY predictVelocity(const POSE& Pose1, const VELOCITY& Vel1, const IMUBIAS& Bias1) const { VELOCITY predictVelocity(const POSE& Pose1, const VELOCITY& Vel1, const IMUBIAS& Bias1) const {
// Calculate the corrected measurements using the Bias object // Calculate the corrected measurements using the Bias object
Vector AccCorrected(Bias1.CorrectAcc(measurement_acc_)); Vector AccCorrected(Bias1.correctAccelerometer(measurement_acc_));
const POSE& world_P1_body = Pose1; const POSE& world_P1_body = Pose1;
const VELOCITY& world_V1_body = Vel1; const VELOCITY& world_V1_body = Vel1;
@ -185,7 +185,7 @@ public:
if(body_P_sensor_) { if(body_P_sensor_) {
Matrix body_R_sensor = body_P_sensor_->rotation().matrix(); Matrix body_R_sensor = body_P_sensor_->rotation().matrix();
Vector GyroCorrected(Bias1.CorrectGyro(measurement_gyro_)); Vector GyroCorrected(Bias1.correctGyroscope(measurement_gyro_));
body_omega_body = body_R_sensor * GyroCorrected; body_omega_body = body_R_sensor * GyroCorrected;
Matrix body_omega_body__cross = skewSymmetric(body_omega_body); Matrix body_omega_body__cross = skewSymmetric(body_omega_body);
body_a_body = body_R_sensor * AccCorrected - body_omega_body__cross * body_omega_body__cross * body_P_sensor_->translation().vector(); body_a_body = body_R_sensor * AccCorrected - body_omega_body__cross * body_omega_body__cross * body_P_sensor_->translation().vector();