adding EKF-SLAM

functions/jacobian.py

+#!/usr/bin/env python3
+# -*- coding: utf-8 -*-
+"""
+Created on Mon Aug  6 09:32:25 2018
+
+@author: william
+jacobian matrices for EKF
+"""
+
+
+import sympy as sp
+from sympy import sin, cos, Matrix, atan2, sqrt
+import numpy as np
+
+'''
+x_t = [x, y, theta]^T #robot state
+u = [v, w]^T #user input
+x = g(x_t, u)
+for motion model 
+'''
+
+x,y,theta,v,w,d_t  = sp.symbols('x, y, theta, v, w, d_t')
+
+g = Matrix([x+v*d_t*cos(theta+w*d_t), y+v*d_t*sin(theta+w*d_t), theta+w*d_t])
+x_t = Matrix([x, y, theta])
+u = Matrix([v, w])
+
+A = g.jacobian(x_t)
+B = g.jacobian(u)
+
+print('A: ', A)
+print('B: ', B)
+
+'''
+x_t = [x, y, theta]^T #robot state
+z = [r, phi, s]^T # measurement 
+z = h(x)
+for measurement model 
+'''
+lx,ly  = sp.symbols('lx, ly')
+
+r = sqrt((lx-x)*(lx-x)+(ly-y)*(ly-y))
+phi = atan2((ly-y), (lx-x))-theta
+h = Matrix([r, phi])
+
+H = h.jacobian(x_t)
+print('H: ', H)

functions/localization_algos/ekf_SLAM.py

+#!/usr/bin/env python3
+# -*- coding: utf-8 -*-
+"""
+Created on Mon Aug  6 10:22:12 2018
+
+@author: william
+"""
+
+import numpy as np
+import math
+from math import cos, sin, atan2, sqrt, pi
+from numpy import dot, arctan
+from numpy.linalg import inv
+import scipy.linalg as linalg
+#import parameters
+from localization_algo_framework import ekf_algo_framework
+#from sympy import *
+
+def rot_mtx(theta):
+    return np.matrix([[cos(theta), -sin(theta)], [sin(theta), cos(theta)]])
+ 
+
+class Simple_EKF(ekf_algo_framework):
+
+    def __init__(self, algo_name):
+        ekf_algo_framework.__init__(self, algo_name)
+
+    def state_init(self):
+        return 0.01*np.matrix(np.identity(2), dtype = float)
+
+    def state_variance_init(self, num_robots):
+        return 0.01*np.matrix(np.identity(2), dtype = float)
+
+    def calculate_trace_state_variance(self, robot_data):
+        [s, orinetations, sigma_s, index] = robot_data
+        trace_state_var = np.trace(sigma_s)
+        return trace_state_var
+
+    def propagation_update(self, robot_data, sensor_data):
+        [s, orinetations, sigma_s, index] = robot_data
+        [measurement_data, sensor_covariance] = sensor_data
+        sigma_odo = sensor_covariance
+
+        delta_t = measurement_data[0]
+        v = measurement_data[1]
+        orinetations[index] = measurement_data[2]
+        self_theta = orinetations[index]
+
+        i = 2*index
+        s[i,0] = s[i,0] + cos(self_theta)*v*delta_t   #x
+        s[i+1,0] = s[i+1,0] + sin(self_theta)*v*delta_t #y
+    
+        Q = sigma_odo
+        P = sigma_s
+
+        W = delta_t*rot_mtx(self_theta)
+        P = P + W*Q*W.getT() # A is identity matrix 
+
+        sigma_s = P
+        return [s, orinetations, sigma_s]
+
+
+
+    def absolute_obser_update(self, robot_data, sensor_data):
+        [s, orinetations, sigma_s, index] = robot_data
+        [measurement_data, sensor_covariance] = sensor_data
+        sigma_ob = sensor_covariance
+        self_theta = orinetations[index]
+        
+        landmark_loc = measurement_data[0] 
+        meas_range = measurement_data[1]  
+        bearing = measurement_data[2] 
+
+        i = 2*index
+        P = sigma_s
+        R = sigma_ob
+        z = np.matrix([meas_range,bearing]).getT()
+        lx = landmark_loc[0]
+        ly = landmark_loc[1]
+        x = s[i,0]
+        y = s[i+1,0]
+
+        z_hat_0 = sqrt((lx-x)*(lx-x)+(ly-y)*(ly-y))
+        z_hat_1 = (atan2((ly-y), (lx-x))-self_theta)%pi
+        if abs(z_hat_1-pi) < abs(z_hat_1):
+            z_hat_1 = z_hat_1-pi
+
+        z_hat = np.matrix([z_hat_0, z_hat_1]).getT()
+        
+        
+        h11 = (-lx + x)/sqrt((lx - x)**2 + (ly - y)**2)
+        h12 = (-ly + y)/sqrt((lx - x)**2 + (ly - y)**2)
+        h21 = -(-ly + y)/((lx - x)**2 + (ly - y)**2)
+        h22 = -(lx - x)/((lx - x)**2 + (ly - y)**2)
+        
+
+        H = np.matrix([[h11, h12],[h21, h22]])
+        sigma_invention = H * P * H.getT() + R # V is a identity matrix
+        K = P*H.getT()*sigma_invention.getI()
+        s[i:i+2, 0] = s[i:i+2, 0] + K*(z - z_hat)
+        P = P - K*H*P 
+
+        sigma_s = P
+        return [s, orinetations, sigma_s]
\ No newline at end of file