Merge pull request #59 from Gavin-Furtado/Experimenting

Docstring in class

Phase 2/filter_py_lib_test/high_level.py

@@ -0,0 +1,21 @@
+import sensor as sn
+
+
+## Sensor Data
+sensor = sn.PositionSensor(noise_mean=0.2, noise_std=1.5, dt=1,sample_size=50)
+position, velocity, acceleration, noise = sensor.data_set()
+
+print(position)
+
+## Step 0 - Initial State ##
+
+
+## Previous State ##
+
+## Step 1 - Predicted State ##
+
+## Step 2 - Measurement from sensor ##
+
+## Step 3 - Kalman Gain ##
+
+## Step 4 - Update measurement & Kalman Gain ##

Phase 2/filter_py_lib_test/kf_test.py

@@ -15,8 +15,6 @@
 '''
 
 ## To do list
-
-# 2. Solve Github problem
 # 3. Code using own style
 # 4. Prepare state matrix
 
@@ -25,27 +23,6 @@
 from numpy.random import randn
 import matplotlib.pyplot as plt
 
-## dummy data - tracking a dog
-def compute_dog_data(measurement_var, process_var, count=1, dt=1):
-    # initial value for position and velocity
-    position, velocity = 0.,1.
-
-    # calculated standard deviation
-    measurement_std = math.sqrt(measurement_var)
-    process_std = math.sqrt(process_var)
-
-    position_data, measurement_data = [], []
-    for i in range(count):
-        # calculating position and veocity
-        v = velocity + (randn() * process_std)
-        position += v*dt
-        position_data.append(position)
-        measurement_data.append(position + randn() * measurement_std)
-
-    return np.array(position_data), np.array(measurement_data)
-
-# print(compute_dog_data(0.5,0.2))
-
 ## dummy data class - tracking a robot
 ## A model of real world object: electronic sensor
 class PositionSensor(object):
@@ -159,10 +136,16 @@ def gaussian_plot(self):
 
 print(position_data[3])
 print(velocity_data[3])
+print(acceleration_data[3])
 
 X = np.array([[position_data[3][0]],[position_data[3][1]],
               [velocity_data[3][0]],[velocity_data[3][1]]])
-print(X.size)
+u = np.array([[acceleration_data[3][0]],
+              [acceleration_data[3][1]]])
+# print(X)
+# print(X.shape)
+# print(u)
+# print(u.shape)
 
 ## Using FilterPy library
 
@@ -208,7 +191,7 @@ def gaussian_plot(self):
 
 ## Step 1 - Predicted State ##
 class Prediction(object):
-    def __init__(self, X_previous, P_previous, dt=2., u=0, w=0, Q=0) -> None:
+    def __init__(self, X_previous, P_previous, u, dt=2., w=0, Q=0) -> None:
         self.X_previous = X_previous
         self.u = u
         self.w = w
@@ -234,8 +217,8 @@ def A_matrix(self):
         return self.A
 
     def B_matrix(self):
-        u_shape = self.u.shape
-
+        u_shape = self.u.shape # 2,1
+        
         if u_shape[0] == 1: 
             self.B = np.array([[0.5*self.dt**2],
                                [self.dt**2]])
@@ -248,13 +231,15 @@ def B_matrix(self):
         return self.B
 
     def X_predicted(self):
-        pass
+        return (self.A_matrix()@self.X_previous) + (self.B_matrix()@self.u) #+ self.w 
 
     def P_predicted(self):
         pass
 
-predict = Prediction(X,0)
-print(predict.A_matrix())
+predict = Prediction(X,0,u) 
+print(predict.X_predicted())
+# print(predict.u)
+print(predict.B_matrix())
 
 ## Step 2 - Measurement from sensor ##
 

Phase 2/filter_py_lib_test/sensor.py

@@ -0,0 +1,115 @@
+'''
+Module that simulates a real world electronic sensor
+
+Author
+------
+Gavin Furtado
+AOCS Engineer
+'''
+import numpy as np
+
+## dummy data class 
+## A model of real world object: electronic sensor
+class PositionSensor(object):
+    '''
+    Represent a real world position sensor/accelerometer that
+    measures position, velocity and accleration of an object. 
+    This sensor has gaussian noise added to its measurements.
+
+    Attributes
+    ----------  
+    position : float
+        Cooridnates of the moving object in x,y dimensions
+        default values = (0,0)
+    
+    velocity : float
+        Velocity of the moving object in x,y dimensions 
+        default values = (0,0) 
+    
+    noise_std : float  
+        Standard deviation of gaussian noise
+        default value = 0
+
+    noise_mean : float
+        Mean value of the gaussian noise
+        default value = 0 
+
+    sample size: int
+        Number of readings/measurements taken by the sensor
+    
+    Methods
+    -------
+    read():
+        Returns the instanteous position, velocity, acceleration of the 
+        moving object and also returns the noise and standard 
+        deviation of noise.
+    
+    data_set():
+        Returns the complete data set based on the requested sample
+        size. It returns the separarte data set of position, velocity,
+        acceleration and noise
+
+    Example
+    -------
+    import sensor as sn
+
+    sensor = sn.PositionSensor(noise_mean=0.2, noise_std=1.5, dt=1,sample_size=50)
+    
+    position, velocity, acceleration, noise = sensor.data_set()
+    '''
+    def __init__(self, initial_position=(0.,0.), initial_velocity=(0.,0.), 
+                 acceleration=(0.2,0.09),dt=0., noise_mean=0., noise_std=0.,
+                 sample_size=0) -> None:
+        self.position = np.array(initial_position)
+        self.velocity = np.array(initial_velocity)
+        self.acceleration = np.array(acceleration)
+        self.dt = np.array(dt)
+        self.noise_mean = noise_mean
+        self.noise_std = noise_std
+        self.sample_size = sample_size
+
+    def read(self):
+        self.velocity += self.acceleration*self.dt  
+        self.position += self.velocity*self.dt 
+
+        noise = np.random.normal(self.noise_mean, self.noise_std,2) 
+        ## Is this error in measurement(R) or error in process (P)  
+        ## Probably error in measurement(R)                                                  
+
+        return self.position + noise, self.velocity + noise, self.acceleration + noise, noise
+        #return self.position + np.random.randn(2) * self.noise_std
+
+    def data_set(self):
+        '''
+        Creates a data set of position, velocity, acceleration and noise.
+        
+        Parameters
+        ----------
+        None
+        
+        Returns
+        -------
+        postion_data : 2-Dim array
+        velocity_data : 2-Dim array
+        acceleration_data : 2-Dim array
+        noise_data : 2-Dim array
+        '''
+        position_data = np.zeros([self.sample_size,2])
+        velocity_data = np.zeros([self.sample_size,2])
+        acceleration_data = np.zeros([self.sample_size,2])
+        noise_data = np.zeros([self.sample_size,2])
+
+        for i in range(self.sample_size):
+            position, velocity, acceleration, noise = self.read()
+            position_data[i] = position
+            velocity_data[i] = velocity
+            acceleration_data[i] = acceleration
+            noise_data[i] = noise
+        return position_data, velocity_data, acceleration_data, noise_data
+
+## Initialise the Sensor
+sensor = PositionSensor(noise_mean=0.2, noise_std=1.5, dt=1,sample_size=50)
+position, velocity, acceleration, noise = sensor.data_set()
+
+print(position)
+