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

Allow initializing with full quaternion. #182

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

Allow initializing with full quaternion. #182

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
2 commits
Select commit Hold shift + click to select a range
67385b6
Allow initializing with full quaternion.
rikba Jul 19, 2018
4959f86
Merge branch 'master' into feature/init_from_orientation
rikba Mar 2, 2023
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
5 changes: 4 additions & 1 deletion msf_updates/cfg/SinglePositionSensor.cfg
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -35,7 +35,10 @@ MISC = gen.const("MISC", int_t, 0x00000002, "misc
gen.add("core_init_filter", bool_t, INIT_FILTER["value"], "call filter init using defined scale", False)

gen.add("position_noise_meas", double_t, MISC["value"], "noise for measurement sensor (std. dev)", 0.01, 0, 10)
gen.add("position_yaw_init", double_t, MISC["value"], "initial value of yaw", 0, -180, 180)
gen.add("position_q_IB_x", double_t, MISC["value"], "initial value q_IB_x", 0.0, -1.0, 1.0)
gen.add("position_q_IB_y", double_t, MISC["value"], "initial value q_IB_y", 0.0, -1.0, 1.0)
gen.add("position_q_IB_z", double_t, MISC["value"], "initial value q_IB_z", 0.0, -1.0, 1.0)
gen.add("position_q_IB_w", double_t, MISC["value"], "initial value q_IB_w", 1.0, -1.0, 1.0)
gen.add("position_delay", double_t, MISC["value"], "fix delay in seconds", 0.02, -2.0, 2.0)
gen.add("position_noise_p_ip", double_t, MISC["value"], "propagation: noise p_ip (std. dev)", 0.0, 0, 10.0)
gen.add("position_fixed_p_ip", bool_t, MISC["value"], "fix calibration state position", False)
Expand Down
12 changes: 6 additions & 6 deletions msf_updates/src/position_msf/position_sensormanager.h
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -109,13 +109,13 @@ class PositionSensorManager : public msf_core::MSF_SensorManagerROS<
v << 0, 0, 0; /// Robot velocity (IMU centered).
w_m << 0, 0, 0; /// Initial angular velocity.

// Set the initial yaw alignment of body to world (the frame in which the
// Set the initial alignment of body to world (the frame in which the
// position sensor measures).
double yawinit = config_.position_yaw_init / 180 * M_PI;
Eigen::Quaterniond yawq(cos(yawinit / 2), 0, 0, sin(yawinit / 2));
yawq.normalize();

q = yawq;
q.x() = config_.position_q_IB_x;
q.y() = config_.position_q_IB_y;
q.z() = config_.position_q_IB_z;
q.w() = config_.position_q_IB_w;
q.normalize();

P.setZero(); // Error state covariance; if zero, a default initialization
// in msf_core is used
Expand Down