Only keep moving filter for orientation.

src/modules/vision_target_estimator/Orientation/KF_orientation_unified.cpp

@@ -34,37 +34,33 @@
 // TODO: rename to KF_position and clean up comments
 
 /**
- * @file KF_position_moving.cpp
- * @brief Filter to estimate the pose of moving targets. State: [pos_rel, vel_uav, bias, acc_target, vel_target]
+ * @file KF_orientation.cpp
+ * @brief Filter to estimate the orientation of moving targets. State: [yaw, yaw_rate]
  *
  * @author Jonas Perolini <[email protected]>
  *
  */
 
 #include "KF_orientation_unified.h"
-#include "python_derivation/generated/syncState.h"
-#include "python_derivation/generated/predictState.h"
-#include "python_derivation/generated/predictCov.h"
-#include "python_derivation/generated/computeInnovCov.h"
 
 namespace vision_target_estimator
 {
 
 void KF_orientation_unified::predictState(float dt)
 {
-	matrix::Vector<float, vtest::State::size> state_updated;
-	sym::Predictstate(dt, _state, &state_updated);
 
-	for (int i = 0; i < vtest::State::size; i++) {
-		_state(i) = matrix::wrap_pi(state_updated(i));
+	matrix::SquareMatrix<float, 2> phi = getPhi(dt);
+	_state = phi * _state;
+
+	for (int i = 0; i < State::size; i++) {
+		_state(i) = matrix::wrap_pi(_state(i));
 	}
 }
 
 void KF_orientation_unified::predictCov(float dt)
 {
-	matrix::Matrix<float, vtest::State::size, vtest::State::size> cov_updated;
-	sym::Predictcov(dt, _state_covariance, &cov_updated);
-	_state_covariance = cov_updated;
+	matrix::SquareMatrix<float, 2> phi = getPhi(dt);
+	_state_covariance = phi * _state_covariance * phi.transpose();
 }
 
 
@@ -82,11 +78,11 @@ bool KF_orientation_unified::update()
 		return false;
 	}
 
-	const matrix::Matrix<float, vtest::State::size, 1> kalmanGain = _state_covariance * _meas_matrix_row_vect / _innov_cov;
+	const matrix::Matrix<float, 2, 1> kalmanGain = _state_covariance * _meas_matrix_row_vect / _innov_cov;
 
 	_state = _state + kalmanGain * _innov;
 
-	for (int i = 0; i < vtest::State::size; i++) {
+	for (int i = 0; i < 2; i++) {
 		_state(i) = matrix::wrap_pi(_state(i));
 	}
 
@@ -98,20 +94,17 @@ bool KF_orientation_unified::update()
 void KF_orientation_unified::syncState(float dt)
 {
 
-	matrix::Vector<float, vtest::State::size> synced_state;
-	sym::Syncstate(dt, _state, &synced_state);
+	matrix::SquareMatrix<float, 2> phi = getPhi(dt);
+	_sync_state = matrix::inv(phi) * _state;
 
-	for (int i = 0; i < vtest::State::size; i++) {
-		_sync_state(i) = matrix::wrap_pi(synced_state(i));
+	for (int i = 0; i < 2; i++) {
+		_sync_state(i) = matrix::wrap_pi(_sync_state(i));
 	}
 }
 
 float KF_orientation_unified::computeInnovCov(float meas_unc)
 {
-	float innov_cov_updated;
-	sym::Computeinnovcov(meas_unc, _state_covariance, _meas_matrix_row_vect.transpose(), &innov_cov_updated);
-	_innov_cov = innov_cov_updated;
-
+	_innov_cov = (_meas_matrix_row_vect.transpose() * _state_covariance * _meas_matrix_row_vect)(0, 0) + meas_unc;
 	return _innov_cov;
 }
 

src/modules/vision_target_estimator/Orientation/KF_orientation_unified.h

@@ -34,8 +34,8 @@
 // TODO: rename to KF_position and clean up comments
 
 /**
- * @file KF_position_moving.h
- * @brief Filter to estimate the pose of moving targets. State: [r, vd, b, at, vt]
+ * @file KF_orientation.h
+ * @brief Filter to estimate the orientation of moving targets. State: [yaw, yaw_rate]
  *
  * @author Jonas Perolini <[email protected]>
  *
@@ -45,12 +45,19 @@
 #include <mathlib/mathlib.h>
 #include <matrix/Matrix.hpp>
 #include <matrix/Vector.hpp>
-#include "python_derivation/generated/state.h"
 
 #pragma once
 
 namespace vision_target_estimator
 {
+
+namespace State
+{
+static constexpr uint8_t yaw{0};
+static constexpr uint8_t yaw_rate{1};
+static constexpr uint8_t size{2};
+};
+
 class KF_orientation_unified
 {
 public:
@@ -71,20 +78,20 @@ class KF_orientation_unified
 	// Backwards state prediciton
 	void syncState(float dt);
 
-	void setH(const matrix::Vector<float, vtest::State::size> &h_meas) {_meas_matrix_row_vect = h_meas;}
+	void setH(const matrix::Vector<float, State::size> &h_meas) {_meas_matrix_row_vect = h_meas;}
 
-	void setState(const matrix::Vector<float, vtest::State::size> &state) {_state = state;}
+	void setState(const matrix::Vector<float, State::size> &state) {_state = state;}
 
-	void setStateVar(const matrix::Vector<float, vtest::State::size> &var)
+	void setStateVar(const matrix::Vector<float, State::size> &var)
 	{
-		const matrix::SquareMatrix<float, vtest::State::size> var_mat = diag(var);
+		const matrix::SquareMatrix<float, State::size> var_mat = diag(var);
 		_state_covariance = var_mat;
 	};
 
-	matrix::Vector<float, vtest::State::size> getState() { return _state;}
-	matrix::Vector<float, vtest::State::size> getStateVar()
+	matrix::Vector<float, State::size> getState() { return _state;}
+	matrix::Vector<float, State::size> getStateVar()
 	{
-		const matrix::SquareMatrix<float, vtest::State::size> var_mat = _state_covariance;
+		const matrix::SquareMatrix<float, State::size> var_mat = _state_covariance;
 		return var_mat.diag();
 	};
 
@@ -99,13 +106,24 @@ class KF_orientation_unified
 
 private:
 
-	matrix::Vector<float, vtest::State::size> _state;
+	matrix::SquareMatrix<float, State::size> getPhi(float dt)
+	{
+
+		float data[State::size * State::size] = {
+			1, dt,
+			0, 1
+		};
+
+		return matrix::SquareMatrix<float, State::size>(data);
+	}
+
+	matrix::Vector<float, State::size> _state;
 
-	matrix::Vector<float, vtest::State::size> _sync_state;
+	matrix::Vector<float, State::size> _sync_state;
 
-	matrix::Vector<float, vtest::State::size> _meas_matrix_row_vect;
+	matrix::Vector<float, State::size> _meas_matrix_row_vect;
 
-	matrix::Matrix<float, vtest::State::size, vtest::State::size> _state_covariance;
+	matrix::Matrix<float, State::size, State::size> _state_covariance;
 
 	float _innov{0.0f}; // residual of last measurement update
 

src/modules/vision_target_estimator/Orientation/VTEOrientation.cpp

@@ -109,13 +109,13 @@ bool VTEOrientation::initEstimator(const float theta_init)
 
 	PX4_INFO("Theta init %.2f", (double)theta_init);
 
-	matrix::Vector<float, vtest::State::size> state_init;
-	// For moving targets, vtest::State::yaw_rate is init to zero, no need to explicitly set it.
-	state_init(vtest::State::yaw) = theta_init;
+	matrix::Vector<float, State::size> state_init;
+	// For moving targets, State::yaw_rate is init to zero, no need to explicitly set it.
+	state_init(State::yaw) = theta_init;
 
-	matrix::Vector<float, vtest::State::size> state_var_init;
+	matrix::Vector<float, State::size> state_var_init;
 
-	for (int i = 0; i < vtest::State::size; i++) {
+	for (int i = 0; i < State::size; i++) {
 		state_var_init(i) = _yaw_unc;
 	}
 
@@ -205,7 +205,7 @@ bool VTEOrientation::processObsVisionOrientation(const fiducial_marker_yaw_repor
 		obs.updated_theta = true;
 		obs.meas_unc_theta = vision_r_theta_unc;
 		obs.meas_theta = vision_r_theta;
-		obs.meas_h_theta(vtest::State::yaw) = 1;
+		obs.meas_h_theta(State::yaw) = 1;
 
 		return true;
 	}
@@ -272,20 +272,17 @@ void VTEOrientation::publishTarget()
 {
 	vision_target_est_orientation_s vision_target_orientation{};
 
-	matrix::Vector<float, vtest::State::size> state = _target_estimator_orientation->getState();
-	matrix::Vector<float, vtest::State::size> state_var = _target_estimator_orientation->getStateVar();
+	matrix::Vector<float, State::size> state = _target_estimator_orientation->getState();
+	matrix::Vector<float, State::size> state_var = _target_estimator_orientation->getStateVar();
 
 	vision_target_orientation.timestamp = _last_predict;
 	vision_target_orientation.orientation_valid = (hrt_absolute_time() - _last_update < target_valid_TIMEOUT_US);
 
-	vision_target_orientation.theta = state(vtest::State::yaw);
-	vision_target_orientation.cov_theta = state_var(vtest::State::yaw);
+	vision_target_orientation.theta = state(State::yaw);
+	vision_target_orientation.cov_theta = state_var(State::yaw);
 
-#if defined(CONFIG_VTEST_MOVING)
-	vision_target_orientation.v_theta = state(vtest::State::yaw_rate);
-	vision_target_orientation.cov_v_theta =  state_var(vtest::State::yaw_rate);
-
-#endif // CONFIG_VTEST_MOVING
+	vision_target_orientation.v_theta = state(State::yaw_rate);
+	vision_target_orientation.cov_v_theta =  state_var(State::yaw_rate);
 
 	_targetOrientationPub.publish(vision_target_orientation);
 }

src/modules/vision_target_estimator/Orientation/VTEOrientation.h

@@ -64,7 +64,6 @@
 #include <lib/conversion/rotation.h>
 #include <lib/geo/geo.h>
 #include "KF_orientation_unified.h"
-#include "python_derivation/generated/state.h"
 
 using namespace time_literals;
 
@@ -133,7 +132,7 @@ class VTEOrientation: public ModuleParams
 		bool updated_theta = false;
 		float meas_theta = 0.f;
 		float meas_unc_theta = 0.f;
-		matrix::Vector<float, vtest::State::size> meas_h_theta;
+		matrix::Vector<float, State::size> meas_h_theta;
 	};
 
 	bool initTargetEstimator();