def driver():
#parameters
xlim = 30.0 #m
ylim = 30.0 #m
sig_r = 0.2 #m
sig_phi = 0.1 #rad
sig_v = 0.15 #m/s
sig_w = 0.1 #rad/s
x0 = -5.0 #m
y0 = 0.0 #m
th0 = 90.0 #deg!!!!
th0 = th0 * math.pi / 180
#alitude does not change
####need to get these parameters instead of using the parameters below.
# Xtr = self.Xtr #from rover/RelPos
# Rtr = self.Rtr #different measurement model. Will come from cell/NED. Make this N?
# Phi_tr = self.Phi_tr #different measurement model. Will come from cell/NED. Make this E?
# M = self.M #not needed for NE measurements. Could use the base?
# V = self.V #velocity comes from integrating accel from imu
# vc = self.vc
# W = self.W #angular velocity comes from integrating angular accel from imu
# wc = self.wc
# Uc = self.Uc #just a combination of the two.
# Time = self.Time #comes from ROS?
#read in given data
path = rospy.get_param('/estimator/path')
given = sio.loadmat(path + '/src/estimator/midterm_data.mat')
Xtr = given['X_tr']
Rtr = given['range_tr']
Phi_tr = given['bearing_tr']
M = given['m'].astype('float')
V = given['v']
vc = given['v_c']
W = given['om']
wc = given['om_c']
Uc = np.squeeze(np.array([vc, wc]))
Time = given['t']
dt = 0.1 #seconds, this correlates with given Time
tf = 30.0 #seconds, this correlates with given Time
#my specified parameters and variables
Sig = np.array([[1.0, 0.0, 0.0],\
[0.0, 1.0, 0.0],\
[0.0, 0.0, 1.0]])
Mu = np.array([[x0], [y0], [th0]])
Sig_hist = []
Mu_hist = []
Ks_hist = []
Z_hist = []
#instantiate classes
mr = Multirotor(sig_v, sig_w, sig_r, sig_phi, dt, M)
viz = Visualizer(xlim, ylim)
eif = Eif(mr.dyn_2d, mr.model_sensor, dt, sig_v, sig_w, sig_r, sig_phi, M)
#loop through algorithm for each time step
for i in range(len(Time[0])):
Om = inv(Sig)
Ks = Om @ Mu
# Ut = mr.get_vel(Uc[:,i]) #use the given velocities
# Zt = mr.model_sensor(Mu) #use the given measurements
Ut = np.array([vc[0][i], wc[0][i]])
Zt = np.array([Rtr[i], Phi_tr[i]])
Ks, Om = eif.ext_info_filter(Ks, Om, Ut, Zt)
Sig = inv(Om)
Mu = Sig @ Ks
# Mu[2] = utils.wrap(Mu[2]) #could be wrapped, but to match true theta, don't
Ks_hist.append(Ks)
Sig_hist.append(Sig)
Mu_hist.append(Mu)
Z_hist.append(Zt)
Ks_hat = np.squeeze(np.array(Ks_hist))
Mu_hat = np.squeeze(np.array(Mu_hist))
Sig_hat = np.squeeze(np.array(Sig_hist))
Z_hat = np.squeeze(np.array(Z_hist))
error = Xtr.T - Mu_hat
error[:, 2] = utils.wrap(error[:, 2])
viz.plotter(Ks_hat, Mu_hat, Xtr, error, Sig_hat, Z_hat, Rtr, Phi_tr, Time)
viz.animator(Mu_hat, Xtr, M)
#loop through algorithm for each time step
Dif = []
for i in range(len(Time[0])):
Om = inv(Sig)
Ks = Om @ Mu
Ut = mr.get_vel(Uc[:,
i]) #to calculate velocities rather than use the given
# Zt = mr.model_sensor(Mu) #to calculate measurements rather than use the given
# Ut = np.array([vc[0][i],wc[0][i]])
Zt = np.array([Rtr[i], Phi_tr[i]])
Ks, Om = eif.ext_info_filter(Ks, Om, Ut, Zt)
Sig = inv(Om)
Mu = Sig @ Ks
# Mu[2] = utils.wrap(Mu[2]) #could be wrapped, but to match true theta, don't
Ks_hist.append(Ks)
Sig_hist.append(Sig)
Mu_hist.append(Mu)
Z_hist.append(Zt)
Ks_hat = np.squeeze(np.array(Ks_hist))
Mu_hat = np.squeeze(np.array(Mu_hist))
Sig_hat = np.squeeze(np.array(Sig_hist))
Z_hat = np.squeeze(np.array(Z_hist))
error = Xtr.T - Mu_hat
error[:, 2] = utils.wrap(error[:, 2])
viz.plotter(Ks_hat, Mu_hat, Xtr, error, Sig_hat, Z_hat, Rtr, Phi_tr, Time)
viz.animator(Mu_hat, Xtr, M)
