32 float xhalf = 0.5f * x;
39 u.i = 0x5f3759df - (u.i >> 1);
41 u.x = u.x * (1.5f - xhalf * u.x * u.x);
48 T recipNorm =
invSqrt (vx * vx + vy * vy + vz * vz);
57 T recipNorm =
invSqrt (q0 * q0 + q1 * q1 + q2 * q2 + q3 * q3);
65 float q0,
float q1,
float q2,
float q3,
66 float _2dx,
float _2dy,
float _2dz,
67 float& rx,
float& ry,
float& rz) {
70 rx = _2dx * (0.5f - q2 * q2 - q3 * q3)
71 + _2dy * (q0 * q3 + q1 * q2)
72 + _2dz * (q1 * q3 - q0 * q2);
73 ry = _2dx * (q1 * q2 - q0 * q3)
74 + _2dy * (0.5
f - q1 * q1 - q3 * q3)
75 + _2dz * (q0 * q1 + q2 * q3);
76 rz = _2dx * (q0 * q2 + q1 * q3)
77 + _2dy * (q2 * q3 - q0 * q1)
78 + _2dz * (0.5f - q1 * q1 - q2 * q2);
83 float q0,
float q1,
float q2,
float q3,
84 float s0,
float s1,
float s2,
float s3,
86 float& w_bx,
float& w_by,
float& w_bz,
87 float& gx,
float& gy,
float& gz)
90 float w_err_x = 2.0f * q0 * s1 - 2.0f * q1 * s0 - 2.0f * q2 * s3 + 2.0f * q3 * s2;
91 float w_err_y = 2.0f * q0 * s2 + 2.0f * q1 * s3 - 2.0f * q2 * s0 - 2.0f * q3 * s1;
92 float w_err_z = 2.0f * q0 * s3 - 2.0f * q1 * s2 + 2.0f * q2 * s1 - 2.0f * q3 * s0;
94 w_bx += w_err_x * dt * zeta;
95 w_by += w_err_y * dt * zeta;
96 w_bz += w_err_z * dt * zeta;
104 float q0,
float q1,
float q2,
float q3,
105 float gx,
float gy,
float gz,
106 float& qDot1,
float& qDot2,
float& qDot3,
float& qDot4)
110 qDot1 = 0.5f * (-q1 * gx - q2 * gy - q3 * gz);
111 qDot2 = 0.5f * (q0 * gx + q2 * gz - q3 * gy);
112 qDot3 = 0.5f * (q0 * gy - q1 * gz + q3 * gx);
113 qDot4 = 0.5f * (q0 * gz + q1 * gy - q2 * gx);
117 float q0,
float q1,
float q2,
float q3,
118 float _2dx,
float _2dy,
float _2dz,
119 float mx,
float my,
float mz,
120 float& s0,
float& s1,
float& s2,
float& s3)
135 s0 += (_2dy * q3 - _2dz * q2) * f0
136 + (-_2dx * q3 + _2dz * q1) * f1
137 + (_2dx * q2 - _2dy * q1) * f2;
138 s1 += (_2dy * q2 + _2dz * q3) * f0
139 + (_2dx * q2 - 2.0
f * _2dy * q1 + _2dz * q0) * f1
140 + (_2dx * q3 - _2dy * q0 - 2.0f * _2dz * q1) * f2;
141 s2 += (-2.0f * _2dx * q2 + _2dy * q1 - _2dz * q0) * f0
142 + (_2dx * q1 + _2dz * q3) * f1
143 + (_2dx * q0 + _2dy * q3 - 2.0f * _2dz * q2) * f2;
144 s3 += (-2.0f * _2dx * q3 + _2dy * q0 + _2dz * q1) * f0
145 + (-_2dx * q0 - 2.0
f * _2dy * q3 + _2dz * q2) * f1
146 + (_2dx * q1 + _2dy * q2) * f2;
150 float q0,
float q1,
float q2,
float q3,
151 float mx,
float my,
float mz,
152 float& _2bxy,
float& _2bz)
158 _2bxy = 4.0f *
sqrt (hx * hx + hy * hy);
165 q0(1.0), q1(0.0), q2(0.0), q3(0.0),
166 w_bx_(0.0), w_by_(0.0), w_bz_(0.0),
176 float gx,
float gy,
float gz,
177 float ax,
float ay,
float az,
178 float mx,
float my,
float mz,
181 float s0, s1, s2, s3;
182 float qDot1, qDot2, qDot3, qDot4;
186 if (!std::isfinite(mx) || !std::isfinite(my) || !std::isfinite(mz))
193 if (!((ax == 0.0
f) && (ay == 0.0
f) && (az == 0.0
f)))
205 s0 = 0.0; s1 = 0.0; s2 = 0.0; s3 = 0.0;
209 addGradientDescentStep(
q0,
q1,
q2,
q3, 0.0, 0.0, -2.0, ax, ay, az, s0, s1, s2, s3);
212 addGradientDescentStep(
q0,
q1,
q2,
q3, _2bxy, 0.0, _2bz, mx, my, mz, s0, s1, s2, s3);
216 addGradientDescentStep(
q0,
q1,
q2,
q3, 0.0, 0.0, 2.0, ax, ay, az, s0, s1, s2, s3);
219 addGradientDescentStep(
q0,
q1,
q2,
q3, _2bxy, 0.0, _2bz, mx, my, mz, s0, s1, s2, s3);
224 addGradientDescentStep(
q0,
q1,
q2,
q3, 0.0, 0.0, 2.0, ax, ay, az, s0, s1, s2, s3);
227 addGradientDescentStep(
q0,
q1,
q2,
q3, 0.0, _2bxy, _2bz, mx, my, mz, s0, s1, s2, s3);
233 compensateGyroDrift(
q0,
q1,
q2,
q3, s0, s1, s2, s3, dt,
zeta_,
w_bx_,
w_by_,
w_bz_, gx, gy, gz);
235 orientationChangeFromGyro(
q0,
q1,
q2,
q3, gx, gy, gz, qDot1, qDot2, qDot3, qDot4);
245 orientationChangeFromGyro(
q0,
q1,
q2,
q3, gx, gy, gz, qDot1, qDot2, qDot3, qDot4);
259 float gx,
float gy,
float gz,
260 float ax,
float ay,
float az,
264 float s0, s1, s2, s3;
265 float qDot1, qDot2, qDot3, qDot4;
268 orientationChangeFromGyro (
q0,
q1,
q2,
q3, gx, gy, gz, qDot1, qDot2, qDot3, qDot4);
271 if (!((ax == 0.0
f) && (ay == 0.0
f) && (az == 0.0
f)))
277 s0 = 0.0; s1 = 0.0; s2 = 0.0; s3 = 0.0;
281 addGradientDescentStep(
q0,
q1,
q2,
q3, 0.0, 0.0, -2.0, ax, ay, az, s0, s1, s2, s3);
285 addGradientDescentStep(
q0,
q1,
q2,
q3, 0.0, 0.0, 2.0, ax, ay, az, s0, s1, s2, s3);
290 addGradientDescentStep(
q0,
q1,
q2,
q3, 0.0, 0.0, 2.0, ax, ay, az, s0, s1, s2, s3);
static void compensateGyroDrift(float q0, float q1, float q2, float q3, float s0, float s1, float s2, float s3, float dt, float zeta, float &w_bx, float &w_by, float &w_bz, float &gx, float &gy, float &gz)
static float invSqrt(float x)
void madgwickAHRSupdate(float gx, float gy, float gz, float ax, float ay, float az, float mx, float my, float mz, float dt)
static void normalizeVector(T &vx, T &vy, T &vz)
static void rotateAndScaleVector(float q0, float q1, float q2, float q3, float _2dx, float _2dy, float _2dz, float &rx, float &ry, float &rz)
void madgwickAHRSupdateIMU(float gx, float gy, float gz, float ax, float ay, float az, float dt)
static void orientationChangeFromGyro(float q0, float q1, float q2, float q3, float gx, float gy, float gz, float &qDot1, float &qDot2, float &qDot3, float &qDot4)
static void compensateMagneticDistortion(float q0, float q1, float q2, float q3, float mx, float my, float mz, float &_2bxy, float &_2bz)
INLINE Rall1d< T, V, S > sqrt(const Rall1d< T, V, S > &arg)
static void normalizeQuaternion(T &q0, T &q1, T &q2, T &q3)
static void addGradientDescentStep(float q0, float q1, float q2, float q3, float _2dx, float _2dy, float _2dz, float mx, float my, float mz, float &s0, float &s1, float &s2, float &s3)
WorldFrame::WorldFrame world_frame_