02 Nov

#### How to add new algorithms to CoLo

There are two python files need to be modified or create :

1. Modify Function/est_algo.py #estimation algorithm class used by robot class
2. Create Function/algo_EKF/your_own_algo.py #you own algo class in the correct format

the algorithm need to be matched with robot class(centralized or distributive)

In Function/est_algo.py:

1. import algo_EKF.your_own_algo
2. in def init(self, algo_name):

add the following in the if-self statement:

elif algo_name == **your_own_algo**:
self.est_algo = algo_EKF.your_own_algo(algo_name)

Create your own algorithm python inside Function/algo_EKF (Function/algo_EKF/your_own_algo.py):

def rot_mtx(theta):
return matrix([[cos(theta), -sin(theta)], [sin(theta), cos(theta)]])

class your_algo():

def __init__(self, algo_name):
self.algo_name = algo_name

def state_veriance_init(self, num_robots):
return sigma_s #retrun right staring state_veriance for the robot

def algo_update(self, robot_data, update_type, sensor_data):
#return [s, orinetations, sigma_s]
[s, orinetations, sigma_s, index] = robot_data
[measurement_data, sensor_covariance] = sensor_data
if update_type == 'propagation update':
return self.propagation_update(s, orinetations, sigma_s, measurement_data, sensor_covariance, index)
elif update_type == 'landmark observation update':
return self.absolute_obser_update(s, orinetations, sigma_s, measurement_data, sensor_covariance, index)
elif update_type == 'relative observation update':
return self.relative_obser_update(s, orinetations, sigma_s, measurement_data, sensor_covariance, index)
else:
print "invalid update"

def propagation_update(self, s, orinetations, sigma_s, input_data, sigma_odo, index):
**[...]**
return [s, orinetations, sigma_s]

def absolute_obser_update(self, s, orinetations, sigma_s, input_data, sigma_ob, index):
**[...]**
return [s, orinetations, sigma_s]

def relative_obser_update(self, s, orinetations, sigma_s, input_data, sigma_ob, index):
**[...]**
return [s, orinetations, sigma_s]