From 20cabdac8970d4e768c144d3998fd3fba884abc2 Mon Sep 17 00:00:00 2001 From: petrinm Date: Mon, 6 Nov 2023 18:26:03 +0200 Subject: [PATCH] Fix loop condition in FLAE-newton implementation --- ahrs/filters/flae.py | 5 +++-- 1 file changed, 3 insertions(+), 2 deletions(-) diff --git a/ahrs/filters/flae.py b/ahrs/filters/flae.py index 1951a7f..2dc58e9 100644 --- a/ahrs/filters/flae.py +++ b/ahrs/filters/flae.py @@ -512,9 +512,10 @@ def estimate(self, acc: np.ndarray, mag: np.ndarray, method: str = 'symbolic') - t2 = -8*np.linalg.det(H.T) t3 = np.linalg.det(W) if method.lower() == 'newton': - lam = lam_old = 1.0 + lam = 1.0 + lam_old = 0.0 i = 0 - while abs(lam_old-lam) > 1e-8 or i <= 30: + while abs(lam_old-lam) > 1e-8 and i < 6: lam_old = lam f = lam**4 + t1*lam**2 + t2*lam + t3 # (eq. 48) fp = 4*lam**3 + 2*t1*lam + t2 # (eq. 50)