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

correctly initialize orientation #115

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

correctly initialize orientation #115

Changes from all commits
Commits
Show all changes
3 commits
Select commit Hold shift + click to select a range
a5ba14d
correctly initialize orientation
otim Jun 8, 2015
2ac6c0b
fixed typo in comment
otim Jun 8, 2015
4bf4dad
renamed yawq to q_wv_yaw
otim Jun 8, 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
15 changes: 11 additions & 4 deletions msf_updates/src/position_pose_msf/position_pose_sensormanager.h
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -183,11 +183,18 @@ class PositionPoseSensorManager : public msf_core::MSF_SensorManagerROS<
// here we take the attitude from the pose sensor and augment it with
// global yaw init.
double yawinit = config_.position_yaw_init / 180 * M_PI;
Eigen::Quaterniond yawq(cos(yawinit / 2), 0, 0, sin(yawinit / 2));
yawq.normalize();
Eigen::Quaterniond q_wv_yaw(cos(yawinit / 2), 0, 0, sin(yawinit / 2));
q_wv_yaw.normalize();

q = yawq;
q_wv = (q * q_ic * q_vc.conjugate()).conjugate();
q_wv = q_wv_yaw.conjugate();

if (q_vc.w() == 1) { // If there is no pose measurement, only apply q_wv.
Copy link

Choose a reason for hiding this comment

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

isn't there any other way to determine this? this seems unsafe

Copy link
Contributor

Choose a reason for hiding this comment

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

probably better to track this with a separate bool.

Copy link
Contributor Author

Choose a reason for hiding this comment

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

should I add a flag in the sensor handler to track if a measurement has arrived?

Copy link
Contributor

Choose a reason for hiding this comment

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

yes, that is a good way of doing this.

Copy link
Contributor Author

Choose a reason for hiding this comment

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

#117

q = q_wv;
} else { // If there is a pose measurement, apply q_ic and q_wv to get initial attitude.
q = (q_ic * q_vc.conjugate() * q_wv).conjugate();
Copy link

Choose a reason for hiding this comment

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

can't really check the math now

Copy link
Contributor

Choose a reason for hiding this comment

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

This is not correct:

(q_ic * q_vc.conjugate() * q_wv).conjugate();

since the last conjugate needs to be applied to q_wv and not to the entire expr.

q_ic * q_vc.conjugate() * q_wv.conjugate();

Copy link
Contributor Author

Choose a reason for hiding this comment

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

From the notation I agree this should be the case. However, I tried many different things and and this was the only way I got this to work. I actually copied this from the initialization of pose_msf, where it is done the same way.

Copy link
Contributor

Choose a reason for hiding this comment

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

yes, my mistake. The entire expression needs to be inverted again, since the state holds q_wi. But my comment wrt. q_wv holds.

Copy link
Contributor

Choose a reason for hiding this comment

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

i.e. you need (q_ic * q_vc.conjugate() * q_wv.conjugate()).conjugate();

Copy link
Contributor Author

Choose a reason for hiding this comment

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

See how q_wv is used during an update:

https://github.com/ethz-asl/ethzasl_msf/blob/master/msf_updates/include/msf_updates/pose_sensor_handler/pose_measurement.h#L313-315

Doesn't this translate to q_err = (q_wv * q_wi * q_ic).conjugate() * z_q_? (q_wi := q)

https://github.com/ethz-asl/ethzasl_msf/blob/master/msf_updates/include/msf_updates/pose_sensor_handler/pose_measurement.h#L304-309

Here I read position_err = z_p_ - (C_wv.transpose() * (-p_wv + p_wi + C_q.transpose() * p_ic)) * L. Note that C_wv = q_wv.conjugate().toRotationMatrix(). (p_wi := p)

Could it be that the notation is off in the case of q_wv?

Copy link
Contributor

Choose a reason for hiding this comment

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

yes, that is indeed a conflict between notation and usage. There are several things that need to be fixed:

  1. in the computation of the residual for orientation, the q_wv needs to be inverted.
  2. in the computation of the residual for position the transpose on C_wv needs to be removed and then the conjugate removed in the computation of C_wv.
  3. The use of C_q in C_q.transpose() * p_ic is also not consistent since there is another wrong conjugate in L299

it seems I overlooked these when I did the big cleanup from the old filter version... :(

Copy link
Contributor Author

Choose a reason for hiding this comment

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

Ok sounds good!
Won't this break something in the calculation of the H matrix as well?
Should I try to fix this in this PR or a separate one?
Thanks for the review and the help to figure this out!

Copy link
Contributor

Choose a reason for hiding this comment

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

yes, you will need to adapt the measurement matrix (H matrix) as well, most likely there is yet another fishy transpose of the conjugate :(

Copy link
Contributor Author

Choose a reason for hiding this comment

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

#116

}

q.normalize();

MSF_WARN_STREAM("q " << STREAMQUAT(q));
MSF_WARN_STREAM("q_wv " << STREAMQUAT(q_wv));
Expand Down