Skip to content

Commit

Permalink
fix: changed residual var usage in sensor calsses
Browse files Browse the repository at this point in the history 
See #11.
  • Loading branch information
@mascheiber @Chris-Bee
mascheiber authored and Chris-Bee committed Aug 29, 2023
1 parent 1a24513 commit 365396b
Show file tree
Hide file tree
Showing 10 changed files with 31 additions and 24 deletions.
10 changes: 6 additions & 4 deletions source/mars/include/mars/sensors/attitude/attitude_sensor_class.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -264,10 +264,11 @@ class AttitudeSensorClass : public UpdateSensorAbsClass
// Orientation
const Eigen::Vector3d rpy_est = mars::Utils::RPYFromRotMat(R_aw * R_wi * R_ib);
const Eigen::Vector2d rp_est(rpy_est(0), rpy_est(1));
const Eigen::Vector2d res = rp_meas - rp_est;
residual_ = Eigen::MatrixXd(rp_est.rows(), 1);
residual_ = rp_meas - rp_est;

// Perform EKF calculations
mars::Ekf ekf(H, R_meas, res, P);
mars::Ekf ekf(H, R_meas, residual_, P);
const Eigen::MatrixXd correction = ekf.CalculateCorrection(&chi2_);
assert(correction.size() == size_of_full_error_state * 1);

Expand Down Expand Up @@ -377,10 +378,11 @@ class AttitudeSensorClass : public UpdateSensorAbsClass
const Eigen::Quaternion<double> q_est =
prior_sensor_state.q_aw_ * prior_core_state.q_wi_ * prior_sensor_state.q_ib_;
const Eigen::Quaternion<double> res_q = q_est.inverse() * q_meas;
const Eigen::Vector3d res = 2 * res_q.vec() / res_q.w();
residual_ = Eigen::MatrixXd(res_q.vec().rows(), 1);
residual_ << (2 * res_q.vec() / res_q.w());

// Perform EKF calculations
mars::Ekf ekf(H, R_meas, res, P);
mars::Ekf ekf(H, R_meas, residual_, P);
const Eigen::MatrixXd correction = ekf.CalculateCorrection(&chi2_);
assert(correction.size() == size_of_full_error_state * 1);

Expand Down
5 changes: 3 additions & 2 deletions source/mars/include/mars/sensors/bodyvel/bodyvel_sensor_class.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -185,10 +185,11 @@ class BodyvelSensorClass : public UpdateSensorAbsClass
// Calculate the residual z = z~ - (estimate)
// Velocity
const Eigen::Vector3d v_est = R_ib.transpose() * R_wi.transpose() * V_wi + R_ib.transpose() * w_wi_skew * P_ib;
const Eigen::Vector3d res = v_meas - v_est;
residual_ = Eigen::MatrixXd(v_est.rows(), 1);
residual_ = v_meas - v_est;

// Perform EKF calculations
mars::Ekf ekf(H, R_meas, res, P);
mars::Ekf ekf(H, R_meas, residual_, P);
const Eigen::MatrixXd correction = ekf.CalculateCorrection(&chi2_);
assert(correction.size() == size_of_full_error_state * 1);

Expand Down
5 changes: 3 additions & 2 deletions source/mars/include/mars/sensors/gps/gps_sensor_class.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -208,10 +208,11 @@ class GpsSensorClass : public UpdateSensorAbsClass
// Calculate the residual z = z~ - (estimate)
// Position
const Eigen::Vector3d p_est = P_gw_w + R_gw_w * (P_wi + R_wi * P_ig);
const Eigen::Vector3d res = p_meas - p_est;
residual_ = Eigen::MatrixXd(p_est.rows(), 1);
residual_ = p_meas - p_est;

// Perform EKF calculations
mars::Ekf ekf(H, R_meas, res, P);
mars::Ekf ekf(H, R_meas, residual_, P);
const Eigen::MatrixXd correction = ekf.CalculateCorrection(&chi2_);
assert(correction.size() == size_of_full_error_state * 1);

Expand Down
6 changes: 3 additions & 3 deletions source/mars/include/mars/sensors/gps_w_vel/gps_w_vel_sensor_class.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -288,11 +288,11 @@ class GpsVelSensorClass : public UpdateSensorAbsClass
const Eigen::Vector3d res_v = v_meas - v_est;

// Combine residuals (vertical)
Eigen::MatrixXd res(res_p.rows() + res_v.rows(), 1);
res << res_p, res_v;
residual_ = Eigen::MatrixXd(res_p.rows() + res_v.rows(), 1);
residual_ << res_p, res_v;

// Perform EKF calculations
mars::Ekf ekf(H, R_meas, res, P);
mars::Ekf ekf(H, R_meas, residual_, P);
const Eigen::MatrixXd correction = ekf.CalculateCorrection(&chi2_);
assert(correction.size() == size_of_full_error_state * 1);

Expand Down
5 changes: 3 additions & 2 deletions source/mars/include/mars/sensors/mag/mag_sensor_class.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -194,10 +194,11 @@ class MagSensorClass : public UpdateSensorAbsClass
// Calculate the residual z = z~ - (estimate)
// Position
const Eigen::Vector3d mag_est = R_im.transpose() * R_wi.transpose() * mag_w;
const Eigen::Vector3d res = mag_meas - mag_est;
residual_ = Eigen::MatrixXd(mag_est.rows(), 1);
residual_ = mag_meas - mag_est;

// Perform EKF calculations
mars::Ekf ekf(H, R_meas, res, P);
mars::Ekf ekf(H, R_meas, residual_, P);
const Eigen::MatrixXd correction = ekf.CalculateCorrection(&chi2_);
assert(correction.size() == size_of_full_error_state * 1);

Expand Down
6 changes: 3 additions & 3 deletions source/mars/include/mars/sensors/pose/pose_sensor_class.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -199,11 +199,11 @@ class PoseSensorClass : public UpdateSensorAbsClass
const Eigen::Vector3d res_r = 2 * res_q.vec() / res_q.w();

// Combine residuals (vertical)
Eigen::MatrixXd res(res_p.rows() + res_r.rows(), 1);
res << res_p, res_r;
residual_ = Eigen::MatrixXd(res_p.rows() + res_r.rows(), 1);
residual_ << res_p, res_r;

// Perform EKF calculations
mars::Ekf ekf(H, R_meas, res, P);
mars::Ekf ekf(H, R_meas, residual_, P);
const Eigen::MatrixXd correction = ekf.CalculateCorrection(&chi2_);
assert(correction.size() == size_of_full_error_state * 1);

Expand Down
5 changes: 3 additions & 2 deletions source/mars/include/mars/sensors/position/position_sensor_class.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -161,10 +161,11 @@ class PositionSensorClass : public UpdateSensorAbsClass
// Calculate the residual z = z~ - (estimate)
// Position
const Eigen::Vector3d p_est = P_wi + R_wi * P_ip;
const Eigen::Vector3d res = p_meas - p_est;
residual_ = Eigen::MatrixXd(p_est.rows(), 1);
residual_ = p_meas - p_est;

// Perform EKF calculations
mars::Ekf ekf(H, R_meas, res, P);
mars::Ekf ekf(H, R_meas, residual_, P);
const Eigen::MatrixXd correction = ekf.CalculateCorrection(&chi2_);
assert(correction.size() == size_of_full_error_state * 1);

Expand Down
5 changes: 3 additions & 2 deletions source/mars/include/mars/sensors/pressure/pressure_sensor_class.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -208,10 +208,11 @@ class PressureSensorClass : public UpdateSensorAbsClass
// Position
const Eigen::Vector3d h_est3 = P_wi + R_wi * P_ip + bias_p;
const Matrix1d_t h_est = I_el3 * h_est3;
const Matrix1d_t res = h_meas - h_est;
residual_ = Eigen::MatrixXd(h_est.rows(), 1);
residual_ = h_meas - h_est;

// Perform EKF calculations
mars::Ekf ekf(H, R_meas, res, P);
mars::Ekf ekf(H, R_meas, residual_, P);
const Eigen::MatrixXd correction = ekf.CalculateCorrection(&chi2_);
assert(correction.size() == size_of_full_error_state * 1);

Expand Down
2 changes: 1 addition & 1 deletion source/mars/include/mars/sensors/update_sensor_abs_class.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -28,7 +28,7 @@ class UpdateSensorAbsClass : public SensorAbsClass
int aux_states_;
int aux_error_states_;
int ref_to_nav_;
Eigen::VectorXd residual_;
Eigen::MatrixXd residual_;
Eigen::VectorXd R_; ///< Measurement noise "squared"
Eigen::MatrixXd F_;
Eigen::MatrixXd H_;
Expand Down
6 changes: 3 additions & 3 deletions source/mars/include/mars/sensors/vision/vision_sensor_class.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -250,11 +250,11 @@ class VisionSensorClass : public UpdateSensorAbsClass
const Eigen::Vector3d res_r = 2 * res_q.vec() / res_q.w();

// Combine residuals (vertical)
Eigen::MatrixXd res(res_p.rows() + res_r.rows(), 1);
res << res_p, res_r;
residual_ = Eigen::MatrixXd(res_p.rows() + res_r.rows(), 1);
residual_ << res_p, res_r;

// Perform EKF calculations
mars::Ekf ekf(H, R_meas, res, P);
mars::Ekf ekf(H, R_meas, residual_, P);
const Eigen::MatrixXd correction = ekf.CalculateCorrection(&chi2_);
assert(correction.size() == size_of_full_error_state * 1);

Expand Down

0 comments on commit 365396b

Please sign in to comment.