Added plots for Kalman filter and fuzzy controller

alexeysp11 · Apr 5, 2021 · bd6ccbd · bd6ccbd
1 parent 8e8c740
commit bd6ccbd
Show file tree

Hide file tree

Showing 26 changed files with 239 additions and 436 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/combine.py b/RTLS/models/combine.py
diff --git a/RTLS/models/kalman_filter.py b/RTLS/models/kalman_filter.py
@@ -1,13 +1,13 @@
-"""
-KALMAN FILTER WITH ADJUSTED PROCESS VARIANCE
-
-I need to make process_var change its value 
-depending on that how large apriori_error is. 
-"""
-
 import numpy as np
 
 class KalmanFilter:
+    """
+    KALMAN FILTER WITH ADJUSTED PROCESS VARIANCE
+
+    `process_var` changes its value depending on that 
+    how large `apriori_error` is. 
+    """
+
     def __init__(self, observations, error):
         self.observations = observations
         self.error = error
@@ -19,7 +19,7 @@ def estimate(self):
 
         time_sec = len(observations)
         array_size = observations.shape
-        sigma = abs_error / 2 
+        sigma = abs_error * 2 
 
         # allocate space for arrays
         aposteri_est = np.ones(array_size) * observations[0]
@@ -45,7 +45,7 @@ def estimate(self):
             """
 
             # time update
-            apriori_est[k] = aposteri_est[k-1]
+            apriori_est[k] = aposteri_est[k-1] # here you can add accelerometer and tachometer data (apriori_est[k] = aposteri_est[k-1] + tachometer*dt + 1/2*accelerometer * dt**2). 
 
             # get number of columns
             cols = len(aposteri_error[0,:])
@@ -76,10 +76,10 @@ def estimate(self):
             # check if every condition is correct!
             if cols == 2:
                 """
-                If aposteri_error[k-1, x] < aposteri_error[k-2, x] then 
-                apriori_error[k, x] = aposteri_error[k-1, x] - process_var[k-1, x].
-                If aposteri_error[k-1, y] < aposteri_error[k-2, y] then 
-                apriori_error[k, y] = aposteri_error[k-1, y] - process_var[k-1, y].
+                If `aposteri_error[k-1, x] < aposteri_error[k-2, x]` then 
+                `apriori_error[k, x] = aposteri_error[k-1, x] - process_var[k-1, x]`.
+                If `aposteri_error[k-1, y] < aposteri_error[k-2, y]` then 
+                `apriori_error[k, y] = aposteri_error[k-1, y] - process_var[k-1, y]`.
                 """
 
                 if k != 1:

diff --git a/RTLS/models/kf_accelerometer.py b/RTLS/models/kf_accelerometer.py
diff --git a/RTLS/models/kf_const_velocity.py b/RTLS/models/kf_const_velocity.py
diff --git a/RTLS/models/kf_gyro.py b/RTLS/models/kf_gyro.py
diff --git a/RTLS/models/kf_position.py b/RTLS/models/kf_position.py
diff --git a/RTLS/models/predict_position.py b/RTLS/models/predict_position.py
diff --git a/RTLS/models/predict_velocity.py b/RTLS/models/predict_velocity.py
diff --git a/RTLS/sensors/__pycache__/accelerometer.cpython-38.pyc b/RTLS/sensors/__pycache__/accelerometer.cpython-38.pyc
diff --git a/RTLS/sensors/__pycache__/gps.cpython-38.pyc b/RTLS/sensors/__pycache__/gps.cpython-38.pyc
diff --git a/RTLS/sensors/__pycache__/sensor.cpython-38.pyc b/RTLS/sensors/__pycache__/sensor.cpython-38.pyc
diff --git a/RTLS/sensors/accelerometer.py b/RTLS/sensors/accelerometer.py
@@ -12,7 +12,6 @@
 """
 
 import sys
-from models.kf_accelerometer import KalmanFilter
 import numpy as np
 import matplotlib.pyplot as plt
 from tabulate import tabulate
@@ -246,33 +245,7 @@ class NonUniformAccel(Accelerometer):
 
 
 def main():
-
-    # CALL KALMAN FILTER FOR 1D SPACE
-    # input initial data
-    kftest_1d = Accelerometer()
-
-    # get initial data and initialize an instance of KalmanFilter 
-    truth_value, init_guess, n_iter = kftest_1d.initdata_1d()
-    kf_1d = kf_accelerometer.KalmanFilter(truth_value, init_guess, n_iter)
-
-    # estimate, print out results and plot all 
-    obs, est = kf_1d.estimate()
-    kftest_1d.print_out(obs, est, kf_1d)
-    kftest_1d.plot_1d(obs, est, kf_1d)
-
-
-    # CALL KALMAN FILTER FOR 2D SPACE
-    # initialize an instance of Accelerometer
-    kftest_2d = Accelerometer()
-
-    # get initial data and initialize an instance of KalmanFilter 
-    truth_value, init_guess, n_iter = kftest_2d.initdata_2d()
-    kf_2d = kf_accelerometer.KalmanFilter(truth_value, init_guess, n_iter)
-
-    # estimate, print out results and plot all 
-    obs, est = kf_2d.estimate()
-    kftest_2d.print_out(obs, est, kf_2d)
-    kftest_2d.plot_2d(obs, est, kf_2d)
+    pass
 
 
 if __name__ == '__main__':