From 3f7dde21adc86401f0bd2456f742836a22ba3bb1 Mon Sep 17 00:00:00 2001 From: arrenglover Date: Thu, 13 Oct 2022 07:50:43 +0000 Subject: [PATCH 1/7] adding untested latency compensation code --- core/.gitignore | 3 +- core/fusion/fusion.cpp | 89 ++++++++++++-------------- core/fusion/fusion.h | 140 +++++++++++++++++++++++++++++++++++------ 3 files changed, 162 insertions(+), 70 deletions(-) diff --git a/core/.gitignore b/core/.gitignore index 01f9cb9f..9aaa97c7 100644 --- a/core/.gitignore +++ b/core/.gitignore @@ -1,2 +1,3 @@ build/ -.vscode/ \ No newline at end of file +.vscode/ +*.code-workspace \ No newline at end of file diff --git a/core/fusion/fusion.cpp b/core/fusion/fusion.cpp index 1eaeec77..ab78277f 100644 --- a/core/fusion/fusion.cpp +++ b/core/fusion/fusion.cpp @@ -6,24 +6,30 @@ bool stateEstimator::initialise(std::vector 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) { @@ -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; } @@ -108,8 +115,11 @@ bool kfEstimator::initialise(std::vector 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); @@ -120,8 +130,11 @@ void kfEstimator::updateFromVelocity(jointName name, jDot velocity, double dt) state[name] = {kf.statePost.at(0), kf.statePost.at(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(0, 0) = measUD; @@ -131,22 +144,23 @@ void kfEstimator::updateFromPosition(jointName name, joint position, double dt) state[name] = {kf.statePost.at(0), kf.statePost.at(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) { @@ -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) @@ -229,8 +228,11 @@ bool constVelKalman::initialise(std::vector 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; @@ -239,8 +241,11 @@ void constVelKalman::updateFromVelocity(jointName name, jDot velocity, double dt state[name] = {kf.statePost.at(0), kf.statePost.at(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; @@ -249,20 +254,21 @@ void constVelKalman::updateFromPosition(jointName name, joint position, double d state[name] = {kf.statePost.at(0), kf.statePost.at(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) { @@ -273,19 +279,4 @@ void constVelKalman::set(skeleton13 pose) kf.statePost.at(3) = 0.0f; } pose_initialised = true; -} - -bool constVelKalman::poseIsInitialised() -{ - return pose_initialised; -} - -skeleton13 constVelKalman::query() -{ - return state; -} - -joint constVelKalman::query(jointName name) -{ - return state[name]; } \ No newline at end of file diff --git a/core/fusion/fusion.h b/core/fusion/fusion.h index 28da994a..b990618d 100644 --- a/core/fusion/fusion.h +++ b/core/fusion/fusion.h @@ -36,28 +36,31 @@ OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. */ namespace hpecore { +// pure integration class stateEstimator { protected: bool pose_initialised{false}; skeleton13 state{0}; skeleton13_vel velocity{0}; std::deque error[13]; + double prev_ts{0.0}; public: virtual bool initialise(std::vector parameters = {}); - virtual void updateFromVelocity(jointName name, jDot velocity, double dt); - virtual void updateFromPosition(jointName name, joint position, double dt); - virtual void updateFromVelocity(skeleton13 velocity, double dt); - virtual void updateFromPosition(skeleton13 position, double dt); - virtual void set(skeleton13 pose); - bool poseIsInitialised(); - skeleton13 query(); - joint query(jointName name); + virtual void updateFromVelocity(jointName name, jDot velocity, double ts); + virtual void updateFromPosition(jointName name, joint position, double ts); + virtual void updateFromVelocity(skeleton13 velocity, double ts); + virtual void updateFromPosition(skeleton13 position, double ts); + virtual void set(skeleton13 pose, double ts); + virtual bool poseIsInitialised(); + virtual skeleton13 query(); + virtual joint query(jointName name); skeleton13_vel queryVelocity(); void setVelocity(skeleton13_vel vel); std::deque* queryError(); }; +// constant position model class kfEstimator : public stateEstimator { private: std::array kf_array; @@ -67,16 +70,17 @@ class kfEstimator : public stateEstimator { public: bool initialise(std::vector parameters) override; - void updateFromVelocity(skeleton13 velocity, double dt) override; - void updateFromVelocity(jointName name, jDot velocity, double dt) override; - void updateFromPosition(jointName name, joint position, double dt) override; - void updateFromPosition(skeleton13 position, double dt) override; - void set(skeleton13 pose) override; - bool poseIsInitialised(); + void updateFromVelocity(skeleton13 velocity, double ts) override; + void updateFromVelocity(jointName name, jDot velocity, double ts) override; + void updateFromPosition(jointName name, joint position, double ts) override; + void updateFromPosition(skeleton13 position, double ts) override; + void set(skeleton13 pose, double ts) override; + skeleton13 query(); joint query(jointName name); }; +// constant velocity model class constVelKalman : public stateEstimator { private: std::array kf_array; @@ -87,14 +91,110 @@ class constVelKalman : public stateEstimator { public: bool initialise(std::vector parameters) override; - void updateFromVelocity(skeleton13 velocity, double dt) override; - void updateFromVelocity(jointName name, jDot velocity, double dt) override; - void updateFromPosition(jointName name, joint position, double dt) override; - void updateFromPosition(skeleton13 position, double dt) override; - void set(skeleton13 pose) override; - bool poseIsInitialised(); + void updateFromVelocity(skeleton13 velocity, double ts) override; + void updateFromVelocity(jointName name, jDot velocity, double ts) override; + void updateFromPosition(jointName name, joint position, double ts) override; + void updateFromPosition(skeleton13 position, double ts) override; + void set(skeleton13 pose, double ts) override; skeleton13 query(); joint query(jointName name); }; +// latency compensation constant position model +class singleJointLatComp { + private: + cv::KalmanFilter kf; + joint vel_accum; + double measU; + double procU; + double prev_pts; + double prev_vts; + + public: + void initialise(double procU, double measU) { + this->procU = procU; + this->measU = measU; + + // init(state size, measurement size) + kf.init(2, 2); + kf.transitionMatrix = (cv::Mat_(2, 2) << 1, 0, 0, 1); + kf.measurementMatrix = (cv::Mat_(2, 2) << 1, 0, 0, 1); + kf.processNoiseCov = (cv::Mat_(2, 2) << procU, 0, 0, procU); + kf.measurementNoiseCov = (cv::Mat_(2, 2) << measU, 0, 0, measU); + } + + void set(joint position, double ts) { + kf.statePost.at(0) = position.u; + kf.statePost.at(1) = position.v; + prev_pts = ts; + prev_vts = ts; + vel_accum = {0, 0}; + } + + void updateFromVelocity(jDot velocity, double ts) { + vel_accum = vel_accum + velocity * (ts - prev_vts); + prev_vts = ts; + } + + void updateFromPosition(joint position, double ts) { + // perform an (asynchronous) update of the position from the previous + // position estimated (including the previous period velocity accumulation) + double dt = ts - prev_pts; + kf.processNoiseCov.at(0, 0) = procU * dt; + kf.processNoiseCov.at(1, 1) = procU * dt; + kf.predict(); + kf.correct((cv::Mat_(2, 1) << position.u, position.v)); + + // add the current period velocity accumulation to the state + kf.statePost.at(0) += vel_accum.u; + kf.statePost.at(1) += vel_accum.v; + vel_accum = {0.0, 0.0}; + } + + joint query() { + return {kf.statePost.at(0) + vel_accum.u, + kf.statePost.at(1) + vel_accum.v}; + } +}; + +class multiJointLatComp : public stateEstimator { + private: + std::array kf_array; + + public: + bool initialise(std::vector parameters) override { + if (parameters.size() != 2) + return false; + for (auto j : kf_array) + j.initialise(parameters[0], parameters[1]); + pose_initialised = true; + return true; + } + + void updateFromVelocity(jointName name, jDot velocity, double ts) override { + kf_array[name].updateFromVelocity(velocity, ts); + state[name] = kf_array[name].query(); + } + + void updateFromVelocity(skeleton13 velocity, double ts) override { + for (auto name : jointNames) + kf_array[name].updateFromVelocity(velocity[name], ts); + } + + void updateFromPosition(jointName name, joint position, double ts) override { + kf_array[name].updateFromPosition(position, ts); + state[name] = kf_array[name].query(); + } + + void updateFromPosition(skeleton13 position, double ts) override { + for (auto name : jointNames) + kf_array[name].updateFromPosition(position[name], ts); + } + + void set(skeleton13 position, double ts) override { + for (auto name : jointNames) + kf_array[name].set(position[name], ts); + } +}; + } // namespace hpecore From b047bbef9cfa028fc2f7b4613f1a7f9a7149eff7 Mon Sep 17 00:00:00 2001 From: arrenglover Date: Thu, 13 Oct 2022 12:53:40 +0000 Subject: [PATCH 2/7] fix downstream linking issue --- core/fusion/fusion.h | 6 +----- 1 file changed, 1 insertion(+), 5 deletions(-) diff --git a/core/fusion/fusion.h b/core/fusion/fusion.h index b990618d..f402adcc 100644 --- a/core/fusion/fusion.h +++ b/core/fusion/fusion.h @@ -75,9 +75,6 @@ class kfEstimator : public stateEstimator { void updateFromPosition(jointName name, joint position, double ts) override; void updateFromPosition(skeleton13 position, double ts) override; void set(skeleton13 pose, double ts) override; - - skeleton13 query(); - joint query(jointName name); }; // constant velocity model @@ -96,8 +93,7 @@ class constVelKalman : public stateEstimator { void updateFromPosition(jointName name, joint position, double ts) override; void updateFromPosition(skeleton13 position, double ts) override; void set(skeleton13 pose, double ts) override; - skeleton13 query(); - joint query(jointName name); + }; // latency compensation constant position model From 13e4e87561a0398692d555d3883c2be30133c1c8 Mon Sep 17 00:00:00 2001 From: Franco Di Pietro Date: Fri, 14 Oct 2022 13:00:02 +0000 Subject: [PATCH 3/7] bug fixes --- core/fusion/fusion.cpp | 21 +++++++++++++++++++++ core/fusion/fusion.h | 24 ++++++------------------ example/movenet/movenet_online.py | 5 ++--- 3 files changed, 29 insertions(+), 21 deletions(-) diff --git a/core/fusion/fusion.cpp b/core/fusion/fusion.cpp index ab78277f..cc7af277 100644 --- a/core/fusion/fusion.cpp +++ b/core/fusion/fusion.cpp @@ -279,4 +279,25 @@ void constVelKalman::set(skeleton13 pose, double ts) kf.statePost.at(3) = 0.0f; } pose_initialised = true; +} + +// ========================================================================== // + +void singleJointLatComp::updateFromPosition(joint position, double ts) +{ + // perform an (asynchronous) update of the position from the previous + // position estimated (including the previous period velocity accumulation) + double dt = ts - prev_pts; + kf.processNoiseCov.at(0, 0) = procU * dt; + kf.processNoiseCov.at(1, 1) = procU * dt; + kf.predict(); + kf.correct((cv::Mat_(2, 1) << position.u, position.v)); + + // add the current period velocity accumulation to the state + //vel_accum.u *= 5; vel_accum.v *= 5; + std::cout << vel_accum.u << " " << vel_accum.v << std::endl; + std::cout.flush(); + kf.statePost.at(0) += vel_accum.u; + kf.statePost.at(1) += vel_accum.v; + vel_accum = {0.0, 0.0}; } \ No newline at end of file diff --git a/core/fusion/fusion.h b/core/fusion/fusion.h index f402adcc..b6bb1dac 100644 --- a/core/fusion/fusion.h +++ b/core/fusion/fusion.h @@ -132,20 +132,7 @@ class singleJointLatComp { prev_vts = ts; } - void updateFromPosition(joint position, double ts) { - // perform an (asynchronous) update of the position from the previous - // position estimated (including the previous period velocity accumulation) - double dt = ts - prev_pts; - kf.processNoiseCov.at(0, 0) = procU * dt; - kf.processNoiseCov.at(1, 1) = procU * dt; - kf.predict(); - kf.correct((cv::Mat_(2, 1) << position.u, position.v)); - - // add the current period velocity accumulation to the state - kf.statePost.at(0) += vel_accum.u; - kf.statePost.at(1) += vel_accum.v; - vel_accum = {0.0, 0.0}; - } + void updateFromPosition(joint position, double ts); joint query() { return {kf.statePost.at(0) + vel_accum.u, @@ -161,9 +148,8 @@ class multiJointLatComp : public stateEstimator { bool initialise(std::vector parameters) override { if (parameters.size() != 2) return false; - for (auto j : kf_array) + for (auto &j : kf_array) j.initialise(parameters[0], parameters[1]); - pose_initialised = true; return true; } @@ -174,7 +160,7 @@ class multiJointLatComp : public stateEstimator { void updateFromVelocity(skeleton13 velocity, double ts) override { for (auto name : jointNames) - kf_array[name].updateFromVelocity(velocity[name], ts); + updateFromVelocity(name, velocity[name], ts); } void updateFromPosition(jointName name, joint position, double ts) override { @@ -184,10 +170,12 @@ class multiJointLatComp : public stateEstimator { void updateFromPosition(skeleton13 position, double ts) override { for (auto name : jointNames) - kf_array[name].updateFromPosition(position[name], ts); + updateFromPosition(name, position[name], ts); } void set(skeleton13 position, double ts) override { + pose_initialised = true; + state = position; for (auto name : jointNames) kf_array[name].set(position[name], ts); } diff --git a/example/movenet/movenet_online.py b/example/movenet/movenet_online.py index a4832d8d..56bc816d 100644 --- a/example/movenet/movenet_online.py +++ b/example/movenet/movenet_online.py @@ -103,13 +103,11 @@ def close(self): def updateModule(self): # synchronous update called every get period seconds. - t0 = datetime.datetime.now() if dev: np_input = cv2.imread(self.files[self.file_counter]) np_input = cv2.cvtColor(np_input, cv2.COLOR_BGR2GRAY) stamp_in = 17.34450 else: - # Preparing input and output image buffers np_input = np.ones((self.image_h, self.image_w), dtype=np.uint8) self.yarp_image.resize(self.image_w, self.image_h) @@ -132,6 +130,7 @@ def updateModule(self): # self.counter += 1 # can be used to interrupt the program self.yarp_image.copy(read_image) + t0 = datetime.datetime.now() input_image = np.copy(np_input) # input_image_resized = np.zeros([1, 3, self.image_h_model, self.image_w_model]) @@ -147,7 +146,7 @@ def updateModule(self): # Predict the pose # pre = self.run_task.predict_online(input_image_resized) pre = self.run_task.predict_online(input_image) - + # Visualize the result # if self.cfg['show_center']: # img = image_show(input_image, pre=pre['joints'], center=pre['center']) From 4a5becc76cf33587eaf8620d025921d92eff8b37 Mon Sep 17 00:00:00 2001 From: Franco Di Pietro Date: Mon, 17 Oct 2022 12:58:10 +0000 Subject: [PATCH 4/7] adding option to not use LC in lc framework --- core/fusion/fusion.cpp | 21 +-------------------- core/fusion/fusion.h | 41 ++++++++++++++++++++++++++++++++++++++--- 2 files changed, 39 insertions(+), 23 deletions(-) diff --git a/core/fusion/fusion.cpp b/core/fusion/fusion.cpp index cc7af277..df7ed217 100644 --- a/core/fusion/fusion.cpp +++ b/core/fusion/fusion.cpp @@ -281,23 +281,4 @@ void constVelKalman::set(skeleton13 pose, double ts) pose_initialised = true; } -// ========================================================================== // - -void singleJointLatComp::updateFromPosition(joint position, double ts) -{ - // perform an (asynchronous) update of the position from the previous - // position estimated (including the previous period velocity accumulation) - double dt = ts - prev_pts; - kf.processNoiseCov.at(0, 0) = procU * dt; - kf.processNoiseCov.at(1, 1) = procU * dt; - kf.predict(); - kf.correct((cv::Mat_(2, 1) << position.u, position.v)); - - // add the current period velocity accumulation to the state - //vel_accum.u *= 5; vel_accum.v *= 5; - std::cout << vel_accum.u << " " << vel_accum.v << std::endl; - std::cout.flush(); - kf.statePost.at(0) += vel_accum.u; - kf.statePost.at(1) += vel_accum.v; - vel_accum = {0.0, 0.0}; -} \ No newline at end of file +// ========================================================================== // \ No newline at end of file diff --git a/core/fusion/fusion.h b/core/fusion/fusion.h index b6bb1dac..0fc0c25e 100644 --- a/core/fusion/fusion.h +++ b/core/fusion/fusion.h @@ -132,7 +132,36 @@ class singleJointLatComp { prev_vts = ts; } - void updateFromPosition(joint position, double ts); + void updateFromPosition(joint position, double ts, bool use_comp = true) + { + //if not using latency compensation we want to integrate before doing + //the position update. + if(!use_comp) + { + kf.statePost.at(0) += vel_accum.u; + kf.statePost.at(1) += vel_accum.v; + vel_accum = {0.0, 0.0}; + } + + // perform an (asynchronous) update of the position from the previous + // position estimated + double dt = ts - prev_pts; + kf.processNoiseCov.at(0, 0) = procU * dt; + kf.processNoiseCov.at(1, 1) = procU * dt; + kf.predict(); + kf.correct((cv::Mat_(2, 1) << position.u, position.v)); + + // add the current period velocity accumulation to the state + // vel_accum.u *= 5; vel_accum.v *= 5; + // std::cout << vel_accum.u << " " << vel_accum.v << std::endl; + //std::cout.flush(); + if (use_comp) + { + kf.statePost.at(0) += vel_accum.u; + kf.statePost.at(1) += vel_accum.v; + vel_accum = {0.0, 0.0}; + } + } joint query() { return {kf.statePost.at(0) + vel_accum.u, @@ -143,16 +172,22 @@ class singleJointLatComp { class multiJointLatComp : public stateEstimator { private: std::array kf_array; + bool use_lc{true}; public: bool initialise(std::vector parameters) override { - if (parameters.size() != 2) + if (parameters.size() < 3) return false; for (auto &j : kf_array) j.initialise(parameters[0], parameters[1]); return true; } + void activateLC(bool use_lc=true) + { + this->use_lc = use_lc; + } + void updateFromVelocity(jointName name, jDot velocity, double ts) override { kf_array[name].updateFromVelocity(velocity, ts); state[name] = kf_array[name].query(); @@ -164,7 +199,7 @@ class multiJointLatComp : public stateEstimator { } void updateFromPosition(jointName name, joint position, double ts) override { - kf_array[name].updateFromPosition(position, ts); + kf_array[name].updateFromPosition(position, ts, use_lc); state[name] = kf_array[name].query(); } From 7d593a3f8df9b230895ea51286495e89d655ad7a Mon Sep 17 00:00:00 2001 From: Franco Di Pietro Date: Tue, 18 Oct 2022 14:59:33 +0000 Subject: [PATCH 5/7] bug fix for undetected joints --- core/fusion/fusion.h | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/core/fusion/fusion.h b/core/fusion/fusion.h index 0fc0c25e..1580438f 100644 --- a/core/fusion/fusion.h +++ b/core/fusion/fusion.h @@ -149,7 +149,8 @@ class singleJointLatComp { kf.processNoiseCov.at(0, 0) = procU * dt; kf.processNoiseCov.at(1, 1) = procU * dt; kf.predict(); - kf.correct((cv::Mat_(2, 1) << position.u, position.v)); + if(position.u > 0 || position.v > 0) + kf.correct((cv::Mat_(2, 1) << position.u, position.v)); // add the current period velocity accumulation to the state // vel_accum.u *= 5; vel_accum.v *= 5; From fb9ec5a249caf79779ba7d59e47d4bf8e6705647 Mon Sep 17 00:00:00 2001 From: arrenglover Date: Tue, 25 Oct 2022 08:20:10 +0000 Subject: [PATCH 6/7] use_lc moved to parameter list --- core/fusion/fusion.h | 8 ++------ 1 file changed, 2 insertions(+), 6 deletions(-) diff --git a/core/fusion/fusion.h b/core/fusion/fusion.h index 1580438f..79e65d30 100644 --- a/core/fusion/fusion.h +++ b/core/fusion/fusion.h @@ -177,18 +177,14 @@ class multiJointLatComp : public stateEstimator { public: bool initialise(std::vector parameters) override { - if (parameters.size() < 3) + if (parameters.size() < 4) return false; for (auto &j : kf_array) j.initialise(parameters[0], parameters[1]); + if(parameters[3] > 0.0) use_lc = true; return true; } - void activateLC(bool use_lc=true) - { - this->use_lc = use_lc; - } - void updateFromVelocity(jointName name, jDot velocity, double ts) override { kf_array[name].updateFromVelocity(velocity, ts); state[name] = kf_array[name].query(); From 24ae946c5f7dc5e496f5dbf7d1ddee7ce5430561 Mon Sep 17 00:00:00 2001 From: arrenglover Date: Tue, 25 Oct 2022 08:21:46 +0000 Subject: [PATCH 7/7] parameter list size using < --- core/fusion/fusion.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/core/fusion/fusion.cpp b/core/fusion/fusion.cpp index df7ed217..a76a25d6 100644 --- a/core/fusion/fusion.cpp +++ b/core/fusion/fusion.cpp @@ -91,7 +91,7 @@ void kfEstimator::setTimePeriod(double dt) bool kfEstimator::initialise(std::vector parameters) { - if (parameters.size() != 3) + if (parameters.size() < 3) return false; procU = parameters[0]; measUD = parameters[1]; @@ -188,7 +188,7 @@ void constVelKalman::setTimePeriod(double dt) bool constVelKalman::initialise(std::vector parameters) { - if (parameters.size() != 4) + if (parameters.size() < 4) return false; procU = parameters[0]; measU = parameters[1];