Repository navigation
feat(holistic_fusion): add a heading factor that is defined for every attitude - #55
Merged
Merged
Conversation
… attitude The yaw factor constrains the Euler yaw of the sensor frame, which is undefined when its x-axis is vertical. The heading factor takes the measured orientation and constrains the rotation about the z-axis of the fixed frame between the estimated and the measured orientation, read from the rotation vector of their offset. Co-Authored-By: Claude Opus 5.5 <noreply@anthropic.com>
nubertj
force-pushed
the
julian/heading_factor
branch
from
October 8, 2026 01:43
ee24031 to
a9120ca
Compare
nubertj
marked this pull request as ready for review
October 8, 2026 20:04
The heading was the rotation about the z-axis of the fixed frame M. With a tilted M, that axis is tilted in the world. The factor then also pulled on the roll and pitch of the state and of R_W_M, which gravity observes. The measured orientation is now brought to the world through R_W_M, and the heading is taken about the z-axis of the world. Co-Authored-By: Claude Opus 5.5 <noreply@anthropic.com>
nubertj
commented
Oct 9, 2026
nubertj
left a comment
nubertj
left a comment
Collaborator
Author
There was a problem hiding this comment.
needed a bit more refienemnt, but was addressed already
nubertj
commented
Oct 9, 2026
nubertj
left a comment
nubertj
left a comment
Collaborator
Author
There was a problem hiding this comment.
generally looking good now.
Name the heading helper after its input, split the measured world rotation into its own line and move the alignment keyframe check above its callers. Co-Authored-By: Claude Opus 5.5 <noreply@anthropic.com>
Open
2 of 3 tasks
This file contains hidden or bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
Sign up for free
to join this conversation on GitHub.
Already have an account?
Sign in to comment
Add this suggestion to a batch that can be applied as a single commit.This suggestion is invalid because no changes were made to the code.Suggestions cannot be applied while the pull request is closed.Suggestions cannot be applied while viewing a subset of changes.Only one suggestion per line can be applied in a batch.Add this suggestion to a batch that can be applied as a single commit.Applying suggestions on deleted lines is not supported.You must change the existing code in this line in order to create a valid suggestion.Outdated suggestions cannot be applied.This suggestion has been applied or marked resolved.Suggestions cannot be applied from pending reviews.Suggestions cannot be applied on multi-line comments.Suggestions cannot be applied while the pull request is queued to merge.Suggestion cannot be applied right now. Please check back later.
Motivation
The yaw factor constrains the Euler yaw
atan2(R(1,0), R(0,0))of the sensor frame S.It is undefined when the x-axis of S is vertical, e.g. a humanoid lying on its back.
Close to that attitude, its Jacobian grows like 1/cos(pitch) and couples with roll.
Main change
GmsfUnaryExpressionAbsoluteHeadingtakes the measured orientationR_M_Smeasin the fixed frame M and constrainse_z · Log(R_W_S * (R_W_M * R_M_Smeas)^-1), the rotation about the z-axis of the world.R_W_M. Roll and pitch stay with gravity, also with a tilted M.Rot2, so it wraps around at ±pi.HolisticFusionHolistic::addUnaryHeadingAbsoluteMeasurementfollows the holistic yaw measurement. It goes throughT_W_Mand has the same alignment keyframe requirement. That check is shared in a helper.Test plan
heading_expression_test: yaw difference for equal tilt (including x-axis vertical), first-order invariance to tilt, measurement brought to the world throughT_W_M, Jacobians of a tilted M that pull only about world z, numerical Jacobians (including x-axis vertical) and the wrap at ±pi.FlexFuserswitched to the heading measurement.🤖 Generated with Claude Code. Co-authored with @nubertj.