From 08ad04c5bf6c317982fdfd4f889c83fa72f0dfdd Mon Sep 17 00:00:00 2001 From: Edvin Date: Tue, 6 May 2025 10:18:35 +0200 Subject: Added Kalman filtering to positon data --- DWM.py | 27 +++++++++++++++++++++++++++ 1 file changed, 27 insertions(+) (limited to 'DWM.py') diff --git a/DWM.py b/DWM.py index 30a7721..9c42303 100755 --- a/DWM.py +++ b/DWM.py @@ -1,5 +1,32 @@ import serial from time import sleep +from filterpy.kalman import KalmanFilter + +x_mu = -0.02163601775523146 +x_std = 0.07074315964054628 +y_mu = 0.02645106742760512 +y_std = 0.07415316805017082 + +kf = KalmanFilter(dim_x=2, dim_z=2) + +# Initial position +kf.x = np.array([0.], + [0.]) + +# State transition matrix +kf.F = np.array([1., 0.], + [0., 1.]) + +# Measurement function +kf.H = np.array([1., 0.], + [0., 1.]) + +# Covariance matrix +kf.P = np.array([x_std**2, 0.], + [0., y_std**2]) + +kf.R = np.array([x_mu, 0.], + [0., y_mu]) updRate = "10 10" # Active Idle -- cgit v1.2.3