RTLS: adaptive KF (added residuals to observations)

alexeysp11 · Apr 27, 2021 · ad39eb1 · ad39eb1
1 parent 4cc05cd
commit ad39eb1
Show file tree

Hide file tree

Showing 4 changed files with 14 additions and 6 deletions.
diff --git a/RTLS/models/__pycache__/kalman_filter.cpython-38.pyc b/RTLS/models/__pycache__/kalman_filter.cpython-38.pyc
diff --git a/RTLS/models/kalman_filter.py b/RTLS/models/kalman_filter.py
@@ -27,8 +27,8 @@ def estimate(self):
         apriori_est = np.ones(array_size) * observations[0]
         apriori_error = np.ones(array_size) * sigma
         kalman_gain = np.zeros(array_size)
-        mes_var = sigma**2 # estimate of measurement variance R 
-        process_var = np.ones(array_size) * sigma # process variance Q
+        mes_var = sigma**2                          # estimate of measurement variance R 
+        process_var = np.ones(array_size) * sigma   # process variance Q
 
         # intial guesses
         aposteri_est[0] = observations[0]
@@ -65,7 +65,11 @@ def estimate(self):
                     apriori_error[k] = aposteri_error[k-1] + process_var[k-1]
 
                 # process_var should not be so large
-                process_var[k] = aposteri_error[k-1] / 2
+                #process_var[k] = aposteri_error[k-1] / 2
+
+                # add residuals to observations
+                v = observations[k] - apriori_est[k]
+                observations[k] = observations[k] + v
 
                 # measurement update
                 kalman_gain[k] = apriori_error[k]/(apriori_error[k] + mes_var)
@@ -100,8 +104,12 @@ def estimate(self):
                     apriori_error[k, 1] = aposteri_error[k-1, 1] + process_var[k-1, 1]
 
                 # process_var should not be so large
-                process_var[k, 0] = aposteri_error[k-1, 0] / 2
-                process_var[k, 1] = aposteri_error[k-1, 1] / 2
+                #process_var[k, 0] = aposteri_error[k-1, 0] / 2
+                #process_var[k, 1] = aposteri_error[k-1, 1] / 2
+
+                # add residuals to observations
+                v = observations[k] - apriori_est[k]
+                observations[k] = observations[k] + v
 
                 # measurement update
                 kalman_gain[k, 0] = apriori_error[k, 0]/(apriori_error[k, 0] + mes_var)

diff --git a/RTLS/sensors/__pycache__/gps.cpython-38.pyc b/RTLS/sensors/__pycache__/gps.cpython-38.pyc
diff --git a/RTLS/sensors/gps.py b/RTLS/sensors/gps.py
@@ -37,7 +37,7 @@ def callkf(self, kf=kf, dimension=1, init_data=0):
         Invokes Kalman filter for 1D and 2D and plots graphs.  
         """
         # usually error of GPS receiver is about 50 meters
-        abs_error = 50.0
+        abs_error = 10.0
 
         try:
             sensor = Sensor()