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

Latency compensation #80

Merged
merged 7 commits into from
Oct 25, 2022
Merged

Latency compensation #80

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
7 commits
Select commit Hold shift + click to select a range
3f7dde2
adding untested latency compensation code
arrenglover Oct 13, 2022
b047bbe
fix downstream linking issue
arrenglover Oct 13, 2022
13e4e87
bug fixes
francodipietro Oct 14, 2022
4a5becc
adding option to not use LC in lc framework
francodipietro Oct 17, 2022
7d593a3
bug fix for undetected joints
francodipietro Oct 18, 2022
fb9ec5a
use_lc moved to parameter list
arrenglover Oct 25, 2022
24ae946
parameter list size using <
arrenglover Oct 25, 2022
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
3 changes: 2 additions & 1 deletion core/.gitignore
View file
Open in desktop
Original file line number Diff line number Diff line change
@@ -1,2 +1,3 @@
build/
.vscode/
.vscode/
*.code-workspace
93 changes: 43 additions & 50 deletions core/fusion/fusion.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -6,24 +6,30 @@ bool stateEstimator::initialise(std::vector<double> parameters)
{
return true;
}
void stateEstimator::updateFromVelocity(jointName name, jDot velocity, double dt)
void stateEstimator::updateFromVelocity(jointName name, jDot velocity, double ts)
{
double dt = ts - prev_ts;
prev_ts = ts;
state[name] += (velocity * dt);
}

void stateEstimator::updateFromPosition(jointName name, joint position, double dt)
void stateEstimator::updateFromPosition(jointName name, joint position, double ts)
{
prev_ts = ts;
state[name] = position;
}

void stateEstimator::updateFromVelocity(skeleton13 velocity, double dt)
void stateEstimator::updateFromVelocity(skeleton13 velocity, double ts)
{
double dt = ts - prev_ts;
prev_ts = ts;
for (int j = 0; j < 13; j++)
state[j] += (velocity[j] * dt);
}

void stateEstimator::updateFromPosition(skeleton13 position, double dt)
void stateEstimator::updateFromPosition(skeleton13 position, double ts)
{
prev_ts = ts;
skeleton13_b valid = jointTest(position);
for (auto &name : jointNames)
{
Expand All @@ -35,8 +41,9 @@ void stateEstimator::updateFromPosition(skeleton13 position, double dt)
// state = position;
}

void stateEstimator::set(skeleton13 pose)
void stateEstimator::set(skeleton13 pose, double ts)
{
prev_ts = ts;
state = pose;
pose_initialised = true;
}
Expand Down Expand Up @@ -84,7 +91,7 @@ void kfEstimator::setTimePeriod(double dt)

bool kfEstimator::initialise(std::vector<double> parameters)
{
if (parameters.size() != 3)
if (parameters.size() < 3)
return false;
procU = parameters[0];
measUD = parameters[1];
Expand All @@ -108,8 +115,11 @@ bool kfEstimator::initialise(std::vector<double> parameters)
return true;
}

void kfEstimator::updateFromVelocity(jointName name, jDot velocity, double dt)
void kfEstimator::updateFromVelocity(jointName name, jDot velocity, double ts)
{
double dt = ts - prev_ts;
prev_ts = ts;

auto &kf = kf_array[name];
joint j_new = state[name] + velocity * dt;
setTimePeriod(dt);
Expand All @@ -120,8 +130,11 @@ void kfEstimator::updateFromVelocity(jointName name, jDot velocity, double dt)
state[name] = {kf.statePost.at<float>(0), kf.statePost.at<float>(1)};
}

void kfEstimator::updateFromPosition(jointName name, joint position, double dt)
void kfEstimator::updateFromPosition(jointName name, joint position, double ts)
{
double dt = ts - prev_ts;
prev_ts = ts;

auto &kf = kf_array[name];
setTimePeriod(dt);
kf.measurementNoiseCov.at<float>(0, 0) = measUD;
Expand All @@ -131,22 +144,23 @@ void kfEstimator::updateFromPosition(jointName name, joint position, double dt)
state[name] = {kf.statePost.at<float>(0), kf.statePost.at<float>(1)};
}

void kfEstimator::updateFromPosition(skeleton13 position, double dt)
void kfEstimator::updateFromPosition(skeleton13 position, double ts)
{
skeleton13_b valid = jointTest(position);
for (auto &name : jointNames)
if(valid[name] && position[name].u && position[name].v)
updateFromPosition(name, position[name], dt);
updateFromPosition(name, position[name], ts);
}

void kfEstimator::updateFromVelocity(skeleton13 velocity, double dt)
void kfEstimator::updateFromVelocity(skeleton13 velocity, double ts)
{
for (auto &name : jointNames)
updateFromVelocity(name, velocity[name], dt);
updateFromVelocity(name, velocity[name], ts);
}

void kfEstimator::set(skeleton13 pose)
void kfEstimator::set(skeleton13 pose, double ts)
{
prev_ts = ts;
state = pose;
for (jointName name : jointNames)
{
Expand All @@ -157,21 +171,6 @@ void kfEstimator::set(skeleton13 pose)
pose_initialised = true;
}

bool kfEstimator::poseIsInitialised()
{
return pose_initialised;
}

skeleton13 kfEstimator::query()
{
return state;
}

joint kfEstimator::query(jointName name)
{
return state[name];
}

// ========================================================================== //

void constVelKalman::setTimePeriod(double dt)
Expand All @@ -189,7 +188,7 @@ void constVelKalman::setTimePeriod(double dt)

bool constVelKalman::initialise(std::vector<double> parameters)
{
if (parameters.size() != 4)
if (parameters.size() < 4)
return false;
procU = parameters[0];
measU = parameters[1];
Expand Down Expand Up @@ -229,8 +228,11 @@ bool constVelKalman::initialise(std::vector<double> parameters)
return true;
}

void constVelKalman::updateFromVelocity(jointName name, jDot velocity, double dt)
void constVelKalman::updateFromVelocity(jointName name, jDot velocity, double ts)
{
double dt = ts - prev_ts;
prev_ts = ts;

auto &kf = kf_array[name];
setTimePeriod(dt);
kf.measurementMatrix = measurement_velocity;
Expand All @@ -239,8 +241,11 @@ void constVelKalman::updateFromVelocity(jointName name, jDot velocity, double dt
state[name] = {kf.statePost.at<float>(0), kf.statePost.at<float>(1)};
}

void constVelKalman::updateFromPosition(jointName name, joint position, double dt)
void constVelKalman::updateFromPosition(jointName name, joint position, double ts)
{
double dt = ts - prev_ts;
prev_ts = ts;

auto &kf = kf_array[name];
setTimePeriod(dt);
kf.measurementMatrix = measurement_position;
Expand All @@ -249,20 +254,21 @@ void constVelKalman::updateFromPosition(jointName name, joint position, double d
state[name] = {kf.statePost.at<float>(0), kf.statePost.at<float>(1)};
}

void constVelKalman::updateFromPosition(skeleton13 position, double dt)
void constVelKalman::updateFromPosition(skeleton13 position, double ts)
{
for (auto &name : jointNames)
updateFromPosition(name, position[name], dt);
updateFromPosition(name, position[name], ts);
}

void constVelKalman::updateFromVelocity(skeleton13 velocity, double dt)
void constVelKalman::updateFromVelocity(skeleton13 velocity, double ts)
{
for (auto &name : jointNames)
updateFromVelocity(name, velocity[name], dt);
updateFromVelocity(name, velocity[name], ts);
}

void constVelKalman::set(skeleton13 pose)
void constVelKalman::set(skeleton13 pose, double ts)
{
prev_ts = ts;
state = pose;
for (jointName name : jointNames)
{
Expand All @@ -275,17 +281,4 @@ void constVelKalman::set(skeleton13 pose)
pose_initialised = true;
}

bool constVelKalman::poseIsInitialised()
{
return pose_initialised;
}

skeleton13 constVelKalman::query()
{
return state;
}

joint constVelKalman::query(jointName name)
{
return state[name];
}
// ========================================================================== //
Loading