35 float xhalf = 0.5f *
x;
42 u.i = 0x5f3759df - (u.i >> 1);
44 u.x = u.x * (1.5f - xhalf * u.x * u.x);
60 T recipNorm =
invSqrt (q0 * q0 + q1 * q1 + q2 * q2 + q3 * q3);
68 float q0,
float q1,
float q2,
float q3,
69 float _2dx,
float _2dy,
float _2dz,
70 float& rx,
float& ry,
float& rz) {
73 rx = _2dx * (0.5f - q2 * q2 - q3 * q3)
74 + _2dy * (q0 * q3 + q1 * q2)
75 + _2dz * (q1 * q3 - q0 * q2);
76 ry = _2dx * (q1 * q2 - q0 * q3)
77 + _2dy * (0.5
f - q1 * q1 - q3 * q3)
78 + _2dz * (q0 * q1 + q2 * q3);
79 rz = _2dx * (q0 * q2 + q1 * q3)
80 + _2dy * (q2 * q3 - q0 * q1)
81 + _2dz * (0.5f - q1 * q1 - q2 * q2);
85 float q0,
float q1,
float q2,
float q3,
86 float gx,
float gy,
float gz,
87 float& qDot1,
float& qDot2,
float& qDot3,
float& qDot4)
91 qDot1 = 0.5f * (-q1 * gx - q2 * gy - q3 * gz);
92 qDot2 = 0.5f * (q0 * gx + q2 * gz - q3 * gy);
93 qDot3 = 0.5f * (q0 * gy - q1 * gz + q3 * gx);
94 qDot4 = 0.5f * (q0 * gz + q1 * gy - q2 * gx);
98 float q0,
float q1,
float q2,
float q3,
99 float _2dx,
float _2dy,
float _2dz,
100 float mx,
float my,
float mz,
101 float& s0,
float& s1,
float& s2,
float& s3)
116 s0 += (_2dy * q3 - _2dz * q2) * f0
117 + (-_2dx * q3 + _2dz * q1) *
f1
118 + (_2dx * q2 - _2dy * q1) *
f2;
119 s1 += (_2dy * q2 + _2dz * q3) * f0
120 + (_2dx * q2 - 2.0
f * _2dy * q1 + _2dz * q0) *
f1
121 + (_2dx * q3 - _2dy * q0 - 2.0f * _2dz * q1) *
f2;
122 s2 += (-2.0f * _2dx * q2 + _2dy * q1 - _2dz * q0) * f0
123 + (_2dx * q1 + _2dz * q3) *
f1
124 + (_2dx * q0 + _2dy * q3 - 2.0f * _2dz * q2) *
f2;
125 s3 += (-2.0f * _2dx * q3 + _2dy * q0 + _2dz * q1) * f0
126 + (-_2dx * q0 - 2.0
f * _2dy * q3 + _2dz * q2) *
f1
127 + (_2dx * q1 + _2dy * q2) *
f2;
134 T& rx,
T& ry,
T& rz) {
161 float Ax =
A[0], Ay =
A[1], Az =
A[2];
164 float Ex =
E[0], Ey =
E[1], Ez =
E[2];
199 R(0,0) = Hx;
R(0,1) = Mx;
R(0,2) = Ax;
200 R(1,0) = Hy;
R(1,1) = My;
R(1,2) = Ay;
201 R(2,0) = Hz;
R(2,1) = Mz;
R(2,2) = Az;
220 if (
fabs(
A[2]) > 0.1) {
224 }
else if (
fabs(
A[0]) > 0.1 ||
fabs(
A[1]) > 0.1) {
238 q0(1.0), q1(0.0), q2(0.0), q3(0.0),
239 w_bx_(0.0), w_by_(0.0), w_bz_(0.0),
241 gain_ (
Parameters::defaultImuFilterMadgwickGain()),
242 zeta_ (
Parameters::defaultImuFilterMadgwickZeta())
280 double recipNorm = 1 /
sqrt(qx * qx + qy * qy +
qz *
qz + qw * qw);
302 double gx,
double gy,
double gz,
303 double ax,
double ay,
double az,
321 float s0, s1, s2, s3;
322 float qDot1, qDot2, qDot3, qDot4;
325 orientationChangeFromGyro (
q0,
q1,
q2,
q3, gx, gy, gz, qDot1, qDot2, qDot3, qDot4);
328 if (!((ax == 0.0
f) && (ay == 0.0
f) && (az == 0.0
f)))
334 s0 = 0.0; s1 = 0.0; s2 = 0.0; s3 = 0.0;
337 addGradientDescentStep(
q0,
q1,
q2,
q3, 0.0, 0.0, 2.0, ax, ay, az, s0, s1, s2, s3);
351 UWARN(
"dt=%f <=0.0, orientation will not be updated!",
dt);