From db41a20170bb7f77959b9901a31582ad2ba93db7 Mon Sep 17 00:00:00 2001 From: Justin Berger Date: Tue, 3 Apr 2018 23:30:47 -0600 Subject: Madgwick code integrated --- include/libsurvive/survive_imu.h | 11 +- src/poser_sba.c | 18 ++-- src/survive_imu.c | 217 ++++++++++++++++++++++++++++++--------- src/survive_vive.c | 2 +- 4 files changed, 188 insertions(+), 60 deletions(-) diff --git a/include/libsurvive/survive_imu.h b/include/libsurvive/survive_imu.h index a37a4df..e7a3d90 100644 --- a/include/libsurvive/survive_imu.h +++ b/include/libsurvive/survive_imu.h @@ -9,14 +9,23 @@ extern "C" { #endif +struct SurviveIMUTracker_p; + typedef struct { FLT updir[3]; - LinmathVec3d current_velocity; // Velocity in world frame + FLT accel_scale_bias; + + LinmathVec3d current_velocity; // Velocity in world frame + LinmathVec3d current_velocity_lp; // Velocity in world frame PoserDataIMU last_data; SurvivePose pose; + LinmathPoint3d pos_lp; SurvivePose lastGT; uint32_t lastGTTime; + + float integralFBx, integralFBy, integralFBz; // integral error terms scaled by Ki + } SurviveIMUTracker; void survive_imu_tracker_set_pose(SurviveIMUTracker *tracker, uint32_t timecode, SurvivePose *pose); diff --git a/src/poser_sba.c b/src/poser_sba.c index 1dbc820..a23eb0f 100644 --- a/src/poser_sba.c +++ b/src/poser_sba.c @@ -272,12 +272,12 @@ static double run_sba_find_3d_structure(SBAData *d, PoserDataLight *pdl, Survive info); // info if (currentPositionValid) { - FLT distp[3]; - sub3d(distp, so->OutPose.Pos, soLocation.Pos); - FLT distance = magnitude3d(distp); - ; - if (distance > 1.) - status = -1; + // FLT distp[3]; + // sub3d(distp, so->OutPose.Pos, soLocation.Pos); + // FLT distance = magnitude3d(distp); + + // if (distance > 1.) + // status = -1; } if (status > 0 && (info[1] / meas_size * 2) < d->max_error) { d->failures_to_reset_cntr = d->failures_to_reset; @@ -296,7 +296,7 @@ static double run_sba_find_3d_structure(SBAData *d, PoserDataLight *pdl, Survive } } - return info[1] / meas_size * 2; + return status; // info[1] / meas_size * 2; } // Optimizes for LH position assuming object is posed at 0 @@ -396,12 +396,12 @@ int PoserSBA(SurviveObject *so, PoserData *pd) { d->failures_to_reset = survive_configi(ctx, "sba-failures-to-reset", SC_GET, 1); d->successes_to_reset_cntr = 0; d->successes_to_reset = survive_configi(ctx, "sba-successes-to-reset", SC_GET, 100); - + d->useIMU = true; d->required_meas = survive_configi(ctx, "sba-required-meas", SC_GET, 8); d->max_error = survive_configf(ctx, "sba-max-error", SC_GET, .0001); d->sensor_time_window = survive_configi(ctx, "sba-time-window", SC_GET, SurviveSensorActivations_default_tolerance * 2); - d->sensor_variance_per_second = survive_configf(ctx, "sba-sensor-variance-per-sec", SC_GET, 0.001); + d->sensor_variance_per_second = survive_configf(ctx, "sba-sensor-variance-per-sec", SC_GET, 10.0); d->sensor_variance = survive_configf(ctx, "sba-sensor-variance", SC_GET, 1.0); d->so = so; diff --git a/src/survive_imu.c b/src/survive_imu.c index 297bbac..cdf254b 100644 --- a/src/survive_imu.c +++ b/src/survive_imu.c @@ -3,13 +3,130 @@ #include "survive_internal.h" #include +//--------------------------------------------------------------------------------------------------- +// Definitions + +#define sampleFreq 240.0f // sample frequency in Hz +#define twoKpDef (2.0f * 0.5f) // 2 * proportional gain +#define twoKiDef (2.0f * 0.0f) // 2 * integral gain + +//--------------------------------------------------------------------------------------------------- +// Function declarations + +float invSqrt(float x) { + float halfx = 0.5f * x; + float y = x; + long i = *(long *)&y; + i = 0x5f3759df - (i >> 1); + y = *(float *)&i; + y = y * (1.5f - (halfx * y * y)); + return y; +} +//--------------------------------------------------------------------------------------------------- +// IMU algorithm update +// From http://x-io.co.uk/open-source-imu-and-ahrs-algorithms/ +static void MahonyAHRSupdateIMU(SurviveIMUTracker *tracker, float gx, float gy, float gz, float ax, float ay, + float az) { + float recipNorm; + float halfvx, halfvy, halfvz; + float halfex, halfey, halfez; + float qa, qb, qc; + + const float twoKp = twoKpDef; // 2 * proportional gain (Kp) + const float twoKi = twoKiDef; // 2 * integral gain (Ki) + + float q0 = tracker->pose.Rot[0]; + float q1 = tracker->pose.Rot[1]; + float q2 = tracker->pose.Rot[2]; + float q3 = tracker->pose.Rot[3]; + + // Compute feedback only if accelerometer measurement valid (avoids NaN in accelerometer normalisation) + if (!((ax == 0.0f) && (ay == 0.0f) && (az == 0.0f))) { + + // Normalise accelerometer measurement + recipNorm = invSqrt(ax * ax + ay * ay + az * az); + ax *= recipNorm; + ay *= recipNorm; + az *= recipNorm; + + // Estimated direction of gravity and vector perpendicular to magnetic flux + halfvx = q1 * q3 - q0 * q2; + halfvy = q0 * q1 + q2 * q3; + halfvz = q0 * q0 - 0.5f + q3 * q3; + + // Error is sum of cross product between estimated and measured direction of gravity + halfex = (ay * halfvz - az * halfvy); + halfey = (az * halfvx - ax * halfvz); + halfez = (ax * halfvy - ay * halfvx); + + // Compute and apply integral feedback if enabled + if (twoKi > 0.0f) { + tracker->integralFBx += twoKi * halfex * (1.0f / sampleFreq); // tracker->integral error scaled by Ki + tracker->integralFBy += twoKi * halfey * (1.0f / sampleFreq); + tracker->integralFBz += twoKi * halfez * (1.0f / sampleFreq); + gx += tracker->integralFBx; // apply tracker->integral feedback + gy += tracker->integralFBy; + gz += tracker->integralFBz; + } else { + tracker->integralFBx = 0.0f; // prevent tracker->integral windup + tracker->integralFBy = 0.0f; + tracker->integralFBz = 0.0f; + } + + // Apply proportional feedback + gx += twoKp * halfex; + gy += twoKp * halfey; + gz += twoKp * halfez; + } + + // Integrate rate of change of quaternion + gx *= (0.5f * (1.0f / sampleFreq)); // pre-multiply common factors + gy *= (0.5f * (1.0f / sampleFreq)); + gz *= (0.5f * (1.0f / sampleFreq)); + qa = q0; + qb = q1; + qc = q2; + q0 += (-qb * gx - qc * gy - q3 * gz); + q1 += (qa * gx + qc * gz - q3 * gy); + q2 += (qa * gy - qb * gz + q3 * gx); + q3 += (qa * gz + qb * gy - qc * gx); + + // Normalise quaternion + recipNorm = invSqrt(q0 * q0 + q1 * q1 + q2 * q2 + q3 * q3); + q0 *= recipNorm; + q1 *= recipNorm; + q2 *= recipNorm; + q3 *= recipNorm; + + tracker->pose.Rot[0] = q0; + tracker->pose.Rot[1] = q1; + tracker->pose.Rot[2] = q2; + tracker->pose.Rot[3] = q3; +} + +static inline uint32_t tick_difference(uint32_t most_recent, uint32_t least_recent) { + uint32_t diff = 0; + if (most_recent > least_recent) { + diff = most_recent - least_recent; + } else { + diff = least_recent - most_recent; + } + + if (diff > 0xFFFFFFFF / 2) + return 0x7FFFFFFF / 2 - diff; + return diff; +} + void survive_imu_tracker_set_pose(SurviveIMUTracker *tracker, uint32_t timecode, SurvivePose *pose) { tracker->pose = *pose; - for (int i = 0; i < 3; i++) - tracker->current_velocity[i] = - (pose->Pos[i] - tracker->lastGT.Pos[i]) / (timecode - tracker->lastGTTime) * 48000000.; + for (int i = 0; i < 3; i++) { + tracker->current_velocity_lp[i] = tracker->current_velocity[i] = 0; + tracker->pos_lp[i] = 0; + } + //(pose->Pos[i] - tracker->lastGT.Pos[i]) / tick_difference(timecode, tracker->lastGTTime) * 48000000.; + tracker->integralFBx = tracker->integralFBy = tracker->integralFBz = 0.0; tracker->lastGTTime = timecode; tracker->lastGT = *pose; } @@ -20,53 +137,54 @@ static void RotateAccel(LinmathVec3d rAcc, const SurvivePose *pose, const Linmat quatrotatevector(rAcc, pose->Rot, accel); LinmathVec3d G = {0, 0, -1}; add3d(rAcc, rAcc, G); - scale3d(rAcc, rAcc, 9.8); + scale3d(rAcc, rAcc, 9.8066); + FLT m = magnitude3d(rAcc); } -static SurvivePose iterate_position(const SurvivePose *pose, const LinmathVec3d vel, double time_diff, - const PoserDataIMU *pIMU) { - SurvivePose result = *pose; +static void iterate_position(SurviveIMUTracker *tracker, double time_diff, const PoserDataIMU *pIMU, FLT *out) { + const SurvivePose *pose = &tracker->pose; + const FLT *vel = tracker->current_velocity; - FLT acc_mul = time_diff * time_diff / 2; - LinmathVec3d rAcc = {0}; - RotateAccel(rAcc, pose, pIMU->accel); + for (int i = 0; i < 3; i++) + out[i] = pose->Pos[i]; - //fprintf(stderr, "r %f %f %f %f\n", pIMU->accel[0], pIMU->accel[1], pIMU->accel[2], quatmagnitude(pIMU->accel)); - //fprintf(stderr, "i %f %f %f %f\n", rAcc[0], rAcc[1], rAcc[2], quatmagnitude(rAcc)); + FLT acc_mul = time_diff * time_diff / 2; + LinmathVec3d acc; + scale3d(acc, pIMU->accel, tracker->accel_scale_bias); + LinmathVec3d rAcc = {0}; + RotateAccel(rAcc, pose, acc); scale3d(rAcc, rAcc, acc_mul); - LinmathVec3d gyro; - for (int i = 0; i < 3; i++) { - result.Pos[i] += time_diff * vel[i] + rAcc[i]; - gyro[i] = time_diff / 2 * pIMU->gyro[i]; + out[i] += time_diff * (vel[i] - tracker->current_velocity_lp[i]); // + rAcc[i]; } - - LinmathEulerAngle curr, next; - quattoeuler(curr, pose->Rot); - add3d(next, curr, gyro); - quatfromeuler(result.Rot, next); - - return result; } -static void iterate_velocity(LinmathVec3d result, const SurvivePose *pose, const LinmathVec3d vel, double time_diff, - PoserDataIMU *pIMU) { +static void iterate_velocity(LinmathVec3d result, SurviveIMUTracker *tracker, double time_diff, PoserDataIMU *pIMU) { + const SurvivePose *pose = &tracker->pose; + const FLT *vel = tracker->current_velocity; scale3d(result, vel, 1.); + + LinmathVec3d acc; + scale3d(acc, pIMU->accel, tracker->accel_scale_bias); + LinmathVec3d rAcc = {0}; - RotateAccel(rAcc, pose, pIMU->accel); + RotateAccel(rAcc, pose, acc); + // fprintf(stderr, "%f\n", time_diff); scale3d(rAcc, rAcc, time_diff); add3d(result, result, rAcc); } void survive_imu_tracker_integrate(SurviveObject *so, SurviveIMUTracker *tracker, PoserDataIMU *data) { if (tracker->last_data.timecode == 0) { + if (tracker->last_data.datamask == imu_calibration_iterations) { tracker->last_data = *data; tracker->pose.Rot[0] = 1.; const FLT up[3] = {0, 0, 1}; quatfrom2vectors(tracker->pose.Rot, tracker->updir, up); + tracker->accel_scale_bias = 1. / magnitude3d(tracker->updir); return; } @@ -82,37 +200,38 @@ void survive_imu_tracker_integrate(SurviveObject *so, SurviveIMUTracker *tracker tracker->updir[i] = data->accel[i] * .10 + tracker->updir[i] * .90; } - const FLT up[3] = {0, 0, 1}; - LinmathQuat upRot, wouldbeUp; - LinmathVec3d rup; - quatrotatevector(rup, tracker->pose.Rot, up); - quatfrom2vectors(upRot, rup, data->accel); + float gx = data->gyro[0], gy = data->gyro[1], gz = data->gyro[2]; + float ax = data->accel[0], ay = data->accel[1], az = data->accel[2]; - quatrotateabout(wouldbeUp, upRot, tracker->pose.Rot); - quatslerp(tracker->pose.Rot, tracker->pose.Rot, wouldbeUp, .1); + MahonyAHRSupdateIMU(tracker, gx, gy, gz, ax, ay, az); - FLT pose_up[3] = {0, 0, 1}; - quatrotatevector(pose_up, tracker->pose.Rot, tracker->updir); + FLT time_diff = tick_difference(data->timecode, tracker->last_data.timecode) / (FLT)so->timebase_hz; - FLT time_diff = (data->timecode - tracker->last_data.timecode) / (FLT)so->timebase_hz; + if (tick_difference(data->timecode, tracker->lastGTTime) < 3200000 * 3) { + FLT next[3]; + iterate_position(tracker, time_diff, data, next); - SurvivePose t_next = iterate_position(&tracker->pose, tracker->current_velocity, time_diff, data); + LinmathVec3d v_next, lp_add; + iterate_velocity(v_next, tracker, time_diff, data); - LinmathVec3d v_next; - iterate_velocity(v_next, &tracker->pose, tracker->current_velocity, time_diff, data); + scale3d(tracker->current_velocity, v_next, 1); - tracker->pose = t_next; + FLT v_alpha = 1; + FLT p_alpha = 1; + scale3d(tracker->current_velocity_lp, tracker->current_velocity_lp, v_alpha); - //fprintf(stderr, "%f %f %f\n", tracker->current_velocity[0], tracker->current_velocity[1], - //tracker->current_velocity[2]); + scale3d(lp_add, v_next, 1 - v_alpha); + add3d(tracker->current_velocity_lp, tracker->current_velocity_lp, lp_add); - scale3d(tracker->current_velocity, v_next, 1); + LinmathPoint3d p_add; + scale3d(p_add, next, 1 - p_alpha); + scale3d(tracker->pos_lp, tracker->pos_lp, p_alpha); + add3d(tracker->pos_lp, tracker->pos_lp, p_add); - tracker->last_data = *data; - - FLT tmp[3]; - ApplyPoseToPoint(tmp, &tracker->pose, up); + for (int i = 0; i < 3; i++) + tracker->pose.Pos[i] = next[i] - tracker->pos_lp[i]; + // scale3d(tracker->pose.Pos, next, 1); + } - printf("[%f, %f, %f] [%f, %f, %f]\n", tracker->pose.Pos[0], tracker->pose.Pos[1], tracker->pose.Pos[2], tmp[0], - tmp[1], tmp[2]); + tracker->last_data = *data; } diff --git a/src/survive_vive.c b/src/survive_vive.c index 53f6d42..1ffb737 100755 --- a/src/survive_vive.c +++ b/src/survive_vive.c @@ -1508,7 +1508,7 @@ void survive_data_cb( SurviveUSBInterface * si ) // FLT conv = (float)((1./deg_per_sec)*(3.14159/180.)) / 8192.; FLT DEGREES_TO_RADS = 3.14159 / 180.; FLT conv = 1. / 10. * DEGREES_TO_RADS; - // calibrate_gyro(obj, agm+3); + calibrate_gyro(obj, agm + 3); agm[3] *= conv; agm[4] *= conv; agm[5] *= conv; -- cgit v1.2.3