Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Jump to bottom

consistent notation #116

Open
wants to merge 7 commits into
base: master
Choose a base branch
from
Open

consistent notation #116

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
7 commits
Select commit Hold shift + click to select a range
434045b
changed usage of q_wv to comply with its notation. changed notation o…
otim Jun 8, 2015
d3057d5
replaced ? : with if else structure
otim Jun 8, 2015
84b094d
addressed simon's comments
otim Jun 8, 2015
bd39f9e
remove conjugate for consistent notation
otim Jun 12, 2015
58b2363
in position_sensormanager it is p_wp not p_vc
otim Jun 12, 2015
e18218a
renamed q_vc to q_pose because attitude measurements can be either q_…
otim Jun 22, 2015
c3b05a9
Merge branch 'master' into fix/consistent_notation
otim Jun 22, 2015
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
104 changes: 56 additions & 48 deletions msf_updates/include/msf_updates/pose_sensor_handler/pose_measurement.h
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -165,17 +165,16 @@ struct PoseMeasurement : public PoseMeasurementBase {
// Get rotation matrices.
Eigen::Matrix<double, 3, 3> C_wv = state.Get<StateQwvIdx>()
.toRotationMatrix();
Eigen::Matrix<double, 3, 3> C_q = state.Get<StateDefinition_T::q>()
Eigen::Matrix<double, 3, 3> C_wi = state.Get<StateDefinition_T::q>()
.toRotationMatrix();

Eigen::Matrix<double, 3, 3> C_ci = state.Get<StateQicIdx>()
.conjugate().toRotationMatrix();

// Preprocess for elements in H matrix.
Eigen::Matrix<double, 3, 1> vecold;

vecold = (-state.Get<StatePwvIdx>() + state.Get<StateDefinition_T::p>()
+ C_q * state.Get<StatePicIdx>()) * state.Get<StateLIdx>();
Eigen::Matrix<double, 3, 1> vecold =
(-state.Get<StatePwvIdx>() + state.Get<StateDefinition_T::p>()
+ C_wi * state.Get<StatePicIdx>()) * state.Get<StateLIdx>();
Eigen::Matrix<double, 3, 3> skewold = Skew(vecold);

Eigen::Matrix<double, 3, 3> pci_sk = Skew(state.Get<StatePicIdx>());
Expand Down Expand Up @@ -224,48 +223,57 @@ struct PoseMeasurement : public PoseMeasurementBase {

// Construct H matrix.
// Position:
H.block<3, 3>(0, kIdxstartcorr_p) = C_wv
H.block<3, 3>(0, kIdxstartcorr_p) = C_wv.transpose()
* state.Get<StateLIdx>()(0); // p

H.block<3, 3>(0, kIdxstartcorr_q) = -C_wv * C_q * pci_sk
H.block<3, 3>(0, kIdxstartcorr_q) = -C_wv.transpose() * C_wi * pci_sk
* state.Get<StateLIdx>()(0); // q

H.block<3, 1>(0, kIdxstartcorr_L) =
scalefix ?
Eigen::Matrix<double, 3, 1>::Zero() :
(C_wv * C_q * state.Get<StatePicIdx>() + C_wv
* (-state.Get<StatePwvIdx>()
+ state.Get<StateDefinition_T::p>())).eval(); // L

H.block<3, 3>(0, kIdxstartcorr_qwv) =
driftwvattfix ?
Eigen::Matrix<double, 3, 3>::Zero() : (-C_wv * skewold).eval(); // q_wv
if (scalefix) {
H.block<3, 1>(0, kIdxstartcorr_L).setZero();
} else {
H.block<3, 1>(0, kIdxstartcorr_L) =
(C_wv.transpose() * C_wi * state.Get<StatePicIdx>() +
C_wv.transpose() *
(-state.Get<StatePwvIdx>() + state.Get<StateDefinition_T::p>())).eval(); // L
}

H.block<3, 3>(0, kIdxstartcorr_pic) =
calibposfix ?
Eigen::Matrix<double, 3, 3>::Zero() :
(C_wv * C_q * state.Get<StateLIdx>()(0)).eval(); //p_ic
if (driftwvattfix) {
H.block<3, 3>(0, kIdxstartcorr_qwv).setZero();
} else {
H.block<3, 3>(0, kIdxstartcorr_qwv) = (-C_wv.transpose() * skewold).eval(); // q_wv
}

if (calibposfix) {
H.block<3, 3>(0, kIdxstartcorr_pic).setZero();
} else {
H.block<3, 3>(0, kIdxstartcorr_pic) =
(C_wv.transpose() * C_wi * state.Get<StateLIdx>()(0)).eval(); //p_ic
}

// TODO (slynen): Check scale commenting
H.block<3, 3>(0, kIdxstartcorr_pwv) =
driftwvposfix ?
Eigen::Matrix<double, 3, 3>::Zero() :
(-Eigen::Matrix<double, 3, 3>::Identity()
/* * state.Get<StateLIdx>()(0)*/).eval(); //p_wv
if (driftwvposfix) {
H.block<3, 3>(0, kIdxstartcorr_pwv).setZero();
} else {
H.block<3, 3>(0, kIdxstartcorr_pwv) = (-Eigen::Matrix<double, 3, 3>::Identity()
/* * state.Get<StateLIdx>()(0)*/).eval(); //p_wv
}

// Attitude.
H.block<3, 3>(3, kIdxstartcorr_q) = C_ci; // q

H.block<3, 3>(3, kIdxstartcorr_qwv) =
driftwvattfix ?
Eigen::Matrix<double, 3, 3>::Zero() :
(C_ci * C_q.transpose()).eval(); // q_wv
if (driftwvattfix) {
H.block<3, 3>(3, kIdxstartcorr_qwv).setZero();
} else {
H.block<3, 3>(3, kIdxstartcorr_qwv) = (C_ci * C_wi.transpose()).eval(); // q_wv
}

H.block<3, 3>(3, kIdxstartcorr_qic) =
calibattfix ?
Eigen::Matrix<double, 3, 3>::Zero() :
Eigen::Matrix<double, 3, 3>::Identity().eval(); //q_ic
if (calibattfix) {
H.block<3, 3>(3, kIdxstartcorr_qic).setZero();
} else {
H.block<3, 3>(3, kIdxstartcorr_qic) =
Eigen::Matrix<double, 3, 3>::Identity().eval(); //q_ic
}

// This line breaks the filter if a position sensor in the global frame is
// available or if we want to set a global yaw rotation.
Expand Down Expand Up @@ -295,22 +303,22 @@ struct PoseMeasurement : public PoseMeasurementBase {
// Get rotation matrices.
Eigen::Matrix<double, 3, 3> C_wv = state.Get<StateQwvIdx>()

.conjugate().toRotationMatrix();
Eigen::Matrix<double, 3, 3> C_q = state.Get<StateDefinition_T::q>()
.conjugate().toRotationMatrix();
.toRotationMatrix();
Eigen::Matrix<double, 3, 3> C_wi = state.Get<StateDefinition_T::q>()
.toRotationMatrix();

// Construct residuals.
// Position.
r_old.block<3, 1>(0, 0) = z_p_
- (C_wv.transpose()
- (C_wv
* (-state.Get<StatePwvIdx>()
+ state.Get<StateDefinition_T::p>()
+ C_q.transpose() * state.Get<StatePicIdx>()))
+ C_wi * state.Get<StatePicIdx>()))
* state.Get<StateLIdx>();

// Attitude.
Eigen::Quaternion<double> q_err;
q_err = (state.Get<StateQwvIdx>()
Eigen::Quaternion<double> q_err; // q_err = \hat{q}_vc * q_cv
q_err = (state.Get<StateQwvIdx>().conjugate()
Copy link
Contributor

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

can you add a comment that this means:

q_err = \hat{q}_vc * q_cv

Copy link

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

isn't here （calculate the residual of position）? p_vc = c_vw*(-p_wv+p_wi + c_wi*pic)L = c_vw(pw_vi + pw_ic)L = c_vw(pw_vc)L = p_vcL (pw_vi means p_vi expressed in the world frame)
is c_wv means the rotation matrix from vision frame to world frame ?
so why use the “C_wv” but not the "C_wv.transpose()" in line 305 ,what's more in function "CalculateH( ) " use" H.block<3, 3>(0, kIdxstartcorr_p) = C_wv.transpose( )" for calculation of the Jacobian

* state.Get<StateDefinition_T::q>()
* state.Get<StateQicIdx>()).conjugate() * z_q_;
r_old.block<3, 1>(3, 0) = q_err.vec() / q_err.w() * 2;
Expand Down Expand Up @@ -388,24 +396,24 @@ struct PoseMeasurement : public PoseMeasurementBase {

//TODO (slynen): check that both measurements have the same states fixed!
Eigen::Matrix<double, 3, 3> C_wv_old, C_wv_new;
Eigen::Matrix<double, 3, 3> C_q_old, C_q_new;
Eigen::Matrix<double, 3, 3> C_wi_old, C_wi_new;

C_wv_new = state_new.Get<StateQwvIdx>().conjugate().toRotationMatrix();
C_q_new = state_new.Get<StateDefinition_T::q>().conjugate()
C_wv_new = state_new.Get<StateQwvIdx>().toRotationMatrix();
C_wi_new = state_new.Get<StateDefinition_T::q>()
.toRotationMatrix();

C_wv_old = state_old.Get<StateQwvIdx>().conjugate().toRotationMatrix();
C_q_old = state_old.Get<StateDefinition_T::q>().conjugate()
C_wv_old = state_old.Get<StateQwvIdx>().toRotationMatrix();
C_wi_old = state_old.Get<StateDefinition_T::q>()
.toRotationMatrix();

// Construct residuals.
// Position:
Eigen::Matrix<double, 3, 1> diffprobpos = (C_wv_new.transpose()
* (-state_new.Get<StatePwvIdx>() + state_new.Get<StateDefinition_T::p>()
+ C_q_new.transpose() * state_new.Get<StatePicIdx>()))
+ C_wi_new * state_new.Get<StatePicIdx>()))
* state_new.Get<StateLIdx>() - (C_wv_old.transpose()
* (-state_old.Get<StatePwvIdx>() + state_old.Get<StateDefinition_T::p>()
+ C_q_old.transpose() * state_old.Get<StatePicIdx>()))
+ C_wi_old * state_old.Get<StatePicIdx>()))
* state_old.Get<StateLIdx>();


Expand Down
3 changes: 3 additions & 0 deletions msf_updates/include/msf_updates/pose_sensor_handler/pose_sensorhandler.h
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -74,6 +74,9 @@ class PoseSensorHandler : public msf_core::SensorHandler<
Eigen::Quaterniond GetAttitudeMeasurement() {
return z_q_;
}
bool IsMeasurementWorldToSensor() const {
return measurement_world_sensor_;
}
//setters for configure values
void SetNoises(double n_zp, double n_zq);
void SetDelay(double delay);
Expand Down
8 changes: 5 additions & 3 deletions msf_updates/include/msf_updates/position_sensor_handler/position_measurement.h
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -136,9 +136,11 @@ struct PositionMeasurement : public PositionMeasurementBase {

H.block<3, 3>(0, idxstartcorr_q_) = -C_q.transpose() * p_prism_imu_sk; // q

H.block<3, 3>(0, idxstartcorr_p_pi_) =
fixed_p_pos_imu ?
Eigen::Matrix<double, 3, 3>::Zero() : (C_q.transpose()).eval(); //p_pos_imu_
if (fixed_p_pos_imu) {
H.block<3, 3>(0, idxstartcorr_p_pi_).setZero();
} else {
H.block<3, 3>(0, idxstartcorr_p_pi_) = (C_q.transpose()).eval(); //p_pos_imu_
}

}

Expand Down
Loading