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]

Next Post Previous Post