Fix possible "Position is NaN"

Timocop · Feb 4, 2024 · e25950f · e25950f
1 parent e65ca2f
commit e25950f
Show file tree

Hide file tree

Showing 2 changed files with 37 additions and 5 deletions.
diff --git a/src/psmoveservice/Filter/PositionFilter.cpp b/src/psmoveservice/Filter/PositionFilter.cpp
@@ -987,7 +987,7 @@ void PositionFilterKalman::update(
 
 			// Update for each dimension
 			for (int i = 0; i < 3; ++i) {
-				if (kal_err_estimate[i] <= k_real_epsilon)
+				if (kal_err_estimate[i] <= k_real_epsilon || (kal_err_estimate[i] + kalman_position_error) <= k_real_epsilon)
 				{
 					kal_err_estimate[i] = kalman_position_error;
 				}

diff --git a/src/psmoveservice/Filter/PositionFilter.h b/src/psmoveservice/Filter/PositionFilter.h
@@ -50,17 +50,31 @@ class PositionFilter : public IPositionFilter
 class PositionFilterPassThru : public PositionFilter
 {
 public:
+	PositionFilterPassThru()
+		: PositionFilter()
+		, m_resetVelocity(false)
+	{
+		lastOpticalFrame = std::chrono::high_resolution_clock::now();
+	}
+
     void update(const t_high_resolution_timepoint timestamp, const PoseFilterPacket &packet) override;
 	t_high_resolution_timepoint lastOpticalFrame;
-	bool m_resetVelocity = false;
+	bool m_resetVelocity;
 };
 
 class PositionFilterLowPassOptical : public PositionFilter
 {
 public:
+	PositionFilterLowPassOptical()
+		: PositionFilter()
+		, m_resetVelocity(false)
+	{
+		lastOpticalFrame = std::chrono::high_resolution_clock::now();
+	}
+
     void update(const t_high_resolution_timepoint timestamp, const PoseFilterPacket &packet) override;
 	t_high_resolution_timepoint lastOpticalFrame;
-	bool m_resetVelocity = false;
+	bool m_resetVelocity;
 };
 
 class PositionFilterLowPassIMU : public PositionFilter
@@ -71,7 +85,6 @@ class PositionFilterLowPassIMU : public PositionFilter
 
 class PositionFilterLowPassExponential : public PositionFilter
 {
-public:
 	void update(const t_high_resolution_timepoint timestamp, const PoseFilterPacket &packet) override;
 	std::list<float> deltaTimeHistory;
 	std::list<Eigen::Vector3f> blendedPositionHistory;
@@ -80,16 +93,35 @@ class PositionFilterLowPassExponential : public PositionFilter
 class PositionFilterComplimentaryOpticalIMU : public PositionFilter
 {
 public:
+	PositionFilterComplimentaryOpticalIMU()
+		: PositionFilter()
+	{
+		lastOpticalFrame = std::chrono::high_resolution_clock::now();
+	}
+
     void update(const t_high_resolution_timepoint timestamp, const PoseFilterPacket &packet) override;
 	t_high_resolution_timepoint lastOpticalFrame;
 };
 
 class PositionFilterKalman : public PositionFilter
 {
 public:
+	PositionFilterKalman()
+		: PositionFilter()
+		, m_resetVelocity(false)
+	{
+		kal_err_estimate[0] = 0.0;
+		kal_err_estimate[1] = 0.0;
+		kal_err_estimate[2] = 0.0;
+
+		kal_current_estimate = Eigen::Vector3f::Zero();
+		kal_gain = Eigen::Vector3f::Zero();
+		lastOpticalFrame = std::chrono::high_resolution_clock::now();
+	}
+
 	void update(const t_high_resolution_timepoint timestamp, const PoseFilterPacket &packet) override;
 	t_high_resolution_timepoint lastOpticalFrame;
-	bool m_resetVelocity = false;
+	bool m_resetVelocity;
 
 private:
 	float kal_err_estimate[3];