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.

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

Implements a Complementary Filter for intelligently fusing yaw from multiple sources #3

Open
wants to merge 22 commits into
base: master
Choose a base branch
from
Open
Show file tree
Hide file tree
Changes from 1 commit
Commits
Show all changes
22 commits
Select commit Hold shift + click to select a range
ff8b68a
Filter successfully implemented, but with rotation unity artifacts. N…
nanospork Aug 4, 2017
057fb92
Filter progress.
nanospork Aug 5, 2017
459fc2d
Filter now fully functional, but source code is still dirty. High-pas…
nanospork Aug 5, 2017
d6607c0
Code cleanup.
nanospork Aug 5, 2017
5ee6a1a
Updated README.md: added information and example for complementary fi…
nanospork Aug 5, 2017
b6418d1
Updated README.md to correct misleading information.
nanospork Aug 6, 2017
f1f4668
Removed unnecessary chrono include.
nanospork Aug 8, 2017
78cda4e
Removed the 'fudging factor' from the wraparound code. May be more pr…
nanospork Aug 8, 2017
93bdd4d
Added workaroud for 'smooth' yaw resets. May need future work.
nanospork Aug 11, 2017
3255ba1
Added 'instantReset' parameter for instantReset, off by default for n…
nanospork Aug 12, 2017
e1e96e4
Added more stringent motion checks and button check to instant yaw re…
nanospork Aug 15, 2017
2593689
Code cleanup
nanospork Aug 15, 2017
584b8ae
Improved angle wrapping solution to remove stutter. Fixed angle wrap …
nanospork Aug 17, 2017
0694057
More descriptive names.
nanospork Aug 18, 2017
6207476
First implementation of 180 degree flip.
nanospork Aug 18, 2017
80d5a26
Fixed flip position offset.
nanospork Aug 18, 2017
28336a4
Fix cmake for linux compilation.
Conzar Aug 21, 2017
ad1b59b
Some cleanup
nanospork Sep 27, 2017
44e49ac
Merge pull request #1 from Conzar/linux-compile
nanospork Dec 1, 2017
ca988cc
Merge pull request #2 from nanospork/flip-180
nanospork Dec 1, 2017
c44b36e
Got manual 'soft' yaw reset (ignores translational coordinate system)…
nanospork Dec 2, 2017
a0ddf1e
Merge pull request #3 from nanospork/yaw_reset
nanospork Jan 28, 2018
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
Next Next commit
Filter successfully implemented, but with rotation unity artifacts. N…
…eed to fix the -180<->+180 problem.
  • Loading branch information
nanospork committed Aug 4, 2017
commit ff8b68a0e0fde300a677bf55ddd156882a4a158d
11 changes: 11 additions & 0 deletions FusionMath.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -26,4 +26,15 @@ namespace je_nourish_fusion {
osvrQuatSetW(quaternion, cos(r / 2) * cos(p / 2) * cos(y / 2) + sin(r / 2) * sin(p / 2) * sin(y / 2));
}

double fixUnityAngle(double angle)
{
if (angle > M_PI * 17/16) {
angle = (2 * M_PI) - angle;
}
else if (angle < -M_PI * 17/16) {
angle = (2 * M_PI) + angle;
}
return angle;
}

}
1 change: 1 addition & 0 deletions FusionMath.h
Original file line number Diff line number Diff line change
Expand Up @@ -4,5 +4,6 @@ namespace je_nourish_fusion {

void rpyFromQuaternion(OSVR_Quaternion* quaternion, OSVR_Vec3* rpy);
void quaternionFromRPY(OSVR_Vec3* rpy, OSVR_Quaternion* quaternion);
double fixUnityAngle(double angle);

}
69 changes: 68 additions & 1 deletion OrientationReader.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -9,7 +9,11 @@ namespace je_nourish_fusion {
if (config.isString()) {
reader = new SingleOrientationReader(ctx, config.asString());
}
if (config.isObject() && config.isMember("roll") && config.isMember("pitch") && config.isMember("yaw")) {
else if (config.isObject() && config.isMember("roll") && config.isMember("pitch") && config.isMember("yawFast")
&& config.isMember("yawAccurate") && config.isMember("alpha")) {
reader = new FilteredOrientationReader(ctx, config);
}
else if (config.isObject() && config.isMember("roll") && config.isMember("pitch") && config.isMember("yaw")) {
reader = new CombinedOrientationReader(ctx, config);
}

Expand All @@ -29,6 +33,15 @@ namespace je_nourish_fusion {
osvrClientGetInterface(ctx, orientation_paths["yaw"].asCString(), &(m_orientations[2]));
}

FilteredOrientationReader::FilteredOrientationReader(OSVR_ClientContext ctx, Json::Value orientation_paths) {
osvrClientGetInterface(ctx, orientation_paths["roll"].asCString(), &(m_orientations[0]));
osvrClientGetInterface(ctx, orientation_paths["pitch"].asCString(), &(m_orientations[1]));
osvrClientGetInterface(ctx, orientation_paths["yawFast"].asCString(), &(m_orientations[2]));
osvrClientGetInterface(ctx, orientation_paths["yawAccurate"].asCString(), &(m_orientations[3]));
// TO DO: Is this a bogus way to read in this value? Do I care?
m_alpha = orientation_paths["alpha"].asDouble();
}

OSVR_ReturnCode CombinedOrientationReader::update(OSVR_OrientationState* orientation, OSVR_TimeValue* timeValue) {
OSVR_OrientationState orientation_x;
OSVR_OrientationState orientation_y;
Expand Down Expand Up @@ -56,4 +69,58 @@ namespace je_nourish_fusion {
return OSVR_RETURN_SUCCESS;
}

OSVR_ReturnCode FilteredOrientationReader::update(OSVR_OrientationState* orientation, OSVR_TimeValue* timeValue) {
OSVR_OrientationState orientation_x;
OSVR_OrientationState orientation_y;
OSVR_OrientationState orientation_z;
OSVR_OrientationState orientation_z2;

OSVR_ReturnCode xret = osvrGetOrientationState(m_orientations[0], timeValue, &orientation_x);
OSVR_ReturnCode yret = osvrGetOrientationState(m_orientations[1], timeValue, &orientation_y);
OSVR_ReturnCode zret = osvrGetOrientationState(m_orientations[2], timeValue, &orientation_z);
OSVR_ReturnCode z2ret = osvrGetOrientationState(m_orientations[3], timeValue, &orientation_z2);

OSVR_Vec3 rpy_x;
OSVR_Vec3 rpy_y;
OSVR_Vec3 rpy_z;
OSVR_Vec3 rpy_z2;

rpyFromQuaternion(&orientation_x, &rpy_x);
rpyFromQuaternion(&orientation_y, &rpy_y);
rpyFromQuaternion(&orientation_z, &rpy_z);
rpyFromQuaternion(&orientation_z2, &rpy_z2);

/*---------------------------------------
// Filter Implementation Here
---------------------------------------*/
double z_fast;
double z_accurate;
double z_out;
double a = m_alpha;
double last_y = m_last_yaw;
double dt = osvr::util::time::duration(*timeValue, *m_last_timeValue);

z_fast = osvrVec3GetZ(&rpy_z);
z_accurate = osvrVec3GetZ(&rpy_z2);

//z_fast = fixUnityAngle(z_fast);
//z_accurate = fixUnityAngle(z_accurate);

z_out = a*(last_y + z_fast*dt) + (1 - a)*(z_accurate);

z_out = fixUnityAngle(z_out);

m_last_yaw = z_out;
m_last_timeValue = timeValue;

OSVR_Vec3 rpy;
osvrVec3SetX(&rpy, osvrVec3GetX(&rpy_x));
osvrVec3SetY(&rpy, osvrVec3GetY(&rpy_y));
osvrVec3SetZ(&rpy, z_out);

quaternionFromRPY(&rpy, orientation);

return OSVR_RETURN_SUCCESS;
}

}
12 changes: 12 additions & 0 deletions OrientationReader.h
Original file line number Diff line number Diff line change
@@ -1,4 +1,5 @@
#include "stdafx.h"
#include <chrono>

namespace je_nourish_fusion {

Expand Down Expand Up @@ -28,4 +29,15 @@ namespace je_nourish_fusion {
OSVR_ClientInterface m_orientations[3];
};

class FilteredOrientationReader : public IOrientationReader {
public:
FilteredOrientationReader(OSVR_ClientContext ctx, Json::Value orientation_paths);
OSVR_ReturnCode update(OSVR_OrientationState* orientation, OSVR_TimeValue* timeValue);
protected:
OSVR_ClientInterface m_orientations[4];
double m_alpha;
double m_last_yaw;
OSVR_TimeValue* m_last_timeValue;
};

}