From 6fa5715252b059d5ff890b36b7c35150adc98ed8 Mon Sep 17 00:00:00 2001
From: paleziart <paleziart@laas.fr>
Date: Wed, 24 Feb 2021 13:56:30 +0100
Subject: [PATCH] Replace inv with pinv in estimator
---
scripts/Estimator.py | 2 +-
1 file changed, 1 insertion(+), 1 deletion(-)
diff --git a/scripts/Estimator.py b/scripts/Estimator.py
index c36f036f..563b909f 100644
--- a/scripts/Estimator.py
+++ b/scripts/Estimator.py
@@ -80,7 +80,7 @@ class KFilter:
# Correct the prediction, using measurement
# Z : measurement vector
- self.K = self.P @ self.H.T @ np.linalg.inv(self.H @ self.P @ self.H.T + self.R)
+ self.K = self.P @ self.H.T @ np.linalg.pinv(self.H @ self.P @ self.H.T + self.R)
self.X = self.X + self.K @ (Z - self.H @ self.X)
self.P = self.P - self.K @ self.H @ self.P
--
GitLab