def ablt_obsv(self, idx, obs_value, landmark): ii = 2 * idx H = sim_env.rot_mtx(self.theta[idx]).getT() * matrix( [[-1, 0], [0, -1]], dtype=float) local_s = self.s[ii:ii + 2] local_sigma = self.sigma[ii:ii + 2, ii:ii + 2] dis = obs_value[0] phi = obs_value[1] hat_z = sim_env.rot_mtx( self.theta[idx]).getT() * (landmark.position + H * local_s) z = matrix([dis * cos(phi), dis * sin(phi)]).getT() sigma_z = sim_env.rot_mtx(phi) * matrix([[ sim_env.var_dis, 0 ], [0, dis * dis * sim_env.var_phi]]) * sim_env.rot_mtx(phi).getT() sigma_invention = H * local_sigma * H.getT() + sigma_z kalman_gain = local_sigma * H.getT() * sigma_invention.getI() self.s[ii:ii + 2] = local_s + kalman_gain * (z - hat_z) self.sigma[ii:ii + 2, ii:ii + 2] = local_sigma - kalman_gain * H * local_sigma
def rela_obsv_update(self, idx, obs_idx, obs_value): i = 2*idx j = 2*obs_idx H_ij = matrix([[0,0,0,0,0,0,0,0,0,0], [0,0,0,0,0,0,0,0,0,0]], dtype=float) H_ij[0, i] = -1 H_ij[1, i+1] = -1 H_ij[0, j] = 1 H_ij[1, j+1] = 1 H = sim_env.rot_mtx(self.theta[idx]).getT()*H_ij dis = obs_value[0] phi = obs_value[1] hat_z = H * self.s z = matrix([dis*cos(phi), dis*sin(phi)]).getT() sigma_z = sim_env.rot_mtx(phi) * matrix([[sim_env.var_dis, 0],[0, dis*dis*sim_env.var_phi]]) * sim_env.rot_mtx(phi).getT() sigma_invention = H * self.sigma * H.getT() + sigma_z kalman_gain = self.sigma*H.getT()*sigma_invention.getI() self.s = self.s + kalman_gain*(z - hat_z) self.sigma = self.sigma - kalman_gain*H*self.sigma
def ablt_obsv_update(self, idx, obs_value, landmark): ii = 2 * idx total_sigma = self.sigma_i + self.sigma_d H_i = matrix( [[0, 0, 0, 0, 0, 0, 0, 0, 0, 0], [0, 0, 0, 0, 0, 0, 0, 0, 0, 0]], dtype=float) H_i[0, ii] = -1 H_i[1, ii + 1] = -1 H = sim_env.rot_mtx(self.theta[idx]).getT() * H_i dis = obs_value[0] phi = obs_value[1] hat_z = sim_env.rot_mtx( self.theta[idx]).getT() * (landmark.position + H_i * self.s) z = matrix([dis * cos(phi), dis * sin(phi)]).getT() sigma_z = sim_env.rot_mtx(phi) * matrix([[ sim_env.var_dis, 0 ], [0, dis * dis * sim_env.var_phi]]) * sim_env.rot_mtx(phi).getT() sigma_invention = H * total_sigma * H.getT() + sigma_z kalman_gain = total_sigma * H.getT() * sigma_invention.getI() self.s = self.s + kalman_gain * (z - hat_z) total_sigma = total_sigma - kalman_gain * H * total_sigma self.sigma_i = (i_mtx_10.copy() - kalman_gain * H) * self.sigma_i * ( i_mtx_10.copy() - kalman_gain * H).getT() + kalman_gain * sigma_z * kalman_gain.getT() self.sigma_d = total_sigma - self.sigma_i
def rela_obsv_update(self, idx, obs_idx, obs_value): ii = 2 * idx jj = 2 * obs_idx sigma_i = self.sigma[ii:ii + 2, ii:ii + 2] sigma_j = self.sigma[jj:jj + 2, jj:jj + 2] dis = obs_value[0] phi = obs_value[1] z = matrix([[dis * cos(phi)], [dis * sin(phi)]]) hat_j = self.s[ii:ii + 2] + sim_env.rot_mtx(self.theta[idx]) * z sigma_z = sim_env.rot_mtx(phi) * matrix([[ sim_env.var_dis, 0 ], [0, dis * dis * sim_env.var_phi]]) * sim_env.rot_mtx(phi).getT() sigma_hat_j = sigma_i + sim_env.rot_mtx( self.theta[idx]) * sigma_z * sim_env.rot_mtx( self.theta[idx]).getT() ci_coeff = 0.83 sigma_j_next_inv = ci_coeff * sigma_j.getI() + ( 1 - ci_coeff) * sigma_hat_j.getI() self.s[jj:jj + 2] = sigma_j_next_inv.getI() * ( ci_coeff * sigma_j.getI() * self.s[jj:jj + 2] + (1 - ci_coeff) * sigma_hat_j.getI() * hat_j) self.sigma[jj:jj + 2, jj:jj + 2] = sigma_j_next_inv.getI()
def rela_obsv_update(self, idx, obs_idx, obs_value): ii = 2 * idx jj = 2 * obs_idx H_ij = matrix( [[0, 0, 0, 0, 0, 0, 0, 0, 0, 0], [0, 0, 0, 0, 0, 0, 0, 0, 0, 0]], dtype=float) H_ij[0, ii] = -1 H_ij[1, ii + 1] = -1 H_ij[0, jj] = 1 H_ij[1, jj + 1] = 1 H = sim_env.rot_mtx(self.theta[idx]).getT() * H_ij dis = obs_value[0] phi = obs_value[1] hat_z = H * self.s z = matrix([dis * cos(phi), dis * sin(phi)]).getT() reduced_sigma = sim_env.z_mtx_10.copy() reduced_sigma[ii:ii + 2, ii:ii + 2] = self.sigma[ii:ii + 2, ii:ii + 2] reduced_sigma[jj:jj + 2, jj:jj + 2] = self.sigma[jj:jj + 2, jj:jj + 2] reduced_sigma[ii:ii + 2, jj:jj + 2] = self.sigma[ii:ii + 2, jj:jj + 2] reduced_sigma[jj:jj + 2, ii:ii + 2] = self.sigma[jj:jj + 2, ii:ii + 2] multi_i = self.sigma[ii:ii + 2, ii:ii + 2].getI() multi_j = self.sigma[jj:jj + 2, jj:jj + 2].getI() sigma_z = sim_env.rot_mtx(phi) * matrix([[ sim_env.var_dis, 0 ], [0, dis * dis * sim_env.var_phi]]) * sim_env.rot_mtx(phi).getT() sigma_invention = H * reduced_sigma * H.getT() + sigma_z kalman_gain = reduced_sigma * H.getT() * sigma_invention.getI() self.s[ii:ii + 2] = self.s[ii:ii + 2] + kalman_gain[ii:ii + 2] * (z - hat_z) self.s[jj:jj + 2] = self.s[jj:jj + 2] + kalman_gain[jj:jj + 2] * (z - hat_z) self.sigma = self.sigma - kalman_gain * H * reduced_sigma multi_i = self.sigma[ii:ii + 2, ii:ii + 2] * multi_i multi_j = self.sigma[jj:jj + 2, jj:jj + 2] * multi_j for k in range(sim_env.N): if (k != idx): if (k != obs_idx): kk = 2 * k self.sigma[ii:ii + 2, kk:kk + 2] = multi_i * self.sigma[ii:ii + 2, kk:kk + 2] self.sigma[jj:jj + 2, kk:kk + 2] = multi_j * self.sigma[jj:jj + 2, kk:kk + 2] self.sigma[kk:kk + 2, ii:ii + 2] = self.sigma[kk:kk + 2, ii:ii + 2] * multi_i.getT() self.sigma[kk:kk + 2, jj:jj + 2] = self.sigma[kk:kk + 2, jj:jj + 2] * multi_j.getT()
def rela_obsv(self, obs_idx, obs_value): i = 2 * self.index j = 2 * obs_idx H_ij = matrix( [[0, 0, 0, 0, 0, 0, 0, 0, 0, 0], [0, 0, 0, 0, 0, 0, 0, 0, 0, 0]], dtype=float) H_ij[0, i] = -1 H_ij[1, i + 1] = -1 H_ij[0, j] = 1 H_ij[1, j + 1] = 1 H = sim_env.rot_mtx(self.theta).getT() * H_ij dis = obs_value[0] phi = obs_value[1] hat_z = H * self.s z = matrix([dis * cos(phi), dis * sin(phi)]).getT() # algorithmic update sigma_z = sim_env.rot_mtx(phi) * matrix([[ sim_env.var_dis, 0 ], [0, dis * dis * sim_env.var_phi]]) * sim_env.rot_mtx(phi).getT() sigma_invention = H * self.sigma * H.getT() + sigma_z kalman_gain = self.sigma * H.getT() * sigma_invention.getI() self.s = self.s + kalman_gain * (z - hat_z) self.sigma = self.sigma - kalman_gain * H * self.sigma # analytic update sigma_th_z = max(sim_env.var_dis, sim_env.d_max * sim_env.d_max * sim_env.var_phi) * sim_env.i_mtx_2.copy() self.th_sigma = (self.th_sigma.getI() + H_ij.getT() * sigma_th_z.getI() * H_ij).getI()
def ablt_obsv(self, obs_value, landmark): i = 2 * self.index H_i = matrix( [[0, 0, 0, 0, 0, 0, 0, 0, 0, 0], [0, 0, 0, 0, 0, 0, 0, 0, 0, 0]], dtype=float) H_i[0, i] = -1 H_i[1, i + 1] = -1 H = sim_env.rot_mtx(self.theta).getT() * H_i dis = obs_value[0] phi = obs_value[1] hat_z = sim_env.rot_mtx( self.theta).getT() * (landmark.position + H_i * self.s) z = matrix([dis * cos(phi), dis * sin(phi)]).getT() # algorithmic update sigma_z = sim_env.rot_mtx(phi) * matrix([[ sim_env.var_dis, 0 ], [0, dis * dis * sim_env.var_phi]]) * sim_env.rot_mtx(phi).getT() sigma_invention = H * self.sigma * H.getT() + sigma_z kalman_gain = self.sigma * H.getT() * sigma_invention.getI() self.s = self.s + kalman_gain * (z - hat_z) self.sigma = self.sigma - kalman_gain * H * self.sigma # analytical update sigma_th_z = max(sim_env.var_dis, sim_env.d_max * sim_env.d_max * sim_env.var_phi) * sim_env.i_mtx_2.copy() self.th_sigma = (self.th_sigma.getI() + H_i.getT() * sigma_th_z.getI() * H_i).getI()
def rela_obsv(self, obs_idx, obs_value): ii = 2*self.index jj = 2*obs_idx H_ij = matrix([[0,0,0,0,0,0,0,0,0,0], [0,0,0,0,0,0,0,0,0,0]], dtype=float) H_ij[0, ii] = -1 H_ij[1, ii+1] = -1 H_ij[0, jj] = 1 H_ij[1, jj+1] = 1 H = sim_env.rot_mtx(self.theta).getT()*H_ij dis = obs_value[0] phi = obs_value[1] hat_z = H * self.s z = matrix([dis*cos(phi), dis*sin(phi)]).getT() sigma_z = sim_env.rot_mtx(phi) * matrix([[sim_env.var_dis, 0],[0, dis*dis*sim_env.var_phi]]) * sim_env.rot_mtx(phi).getT() e = 0.83# (iii+1)*0.01 p_1 = (1/e) * self.sigma_d + self.sigma_i p_2 = (1/(1-e)) * H * (self.sigma_i + self.sigma_d) * H.getT() + sigma_z sigma_invention = H * p_1 * H.getT() + p_2 kalman_gain = p_1 *H.getT()*sigma_invention.getI() self.s = self.s + kalman_gain*(z - hat_z) total_sigma = (sim_env.i_mtx_10.copy()-kalman_gain*H) * p_1 self.sigma_i = (sim_env.i_mtx_10.copy()-kalman_gain*H) * self.sigma_i * (sim_env.i_mtx_10.copy()-kalman_gain*H).getT() + kalman_gain * sigma_z * kalman_gain.getT() self.sigma_d = total_sigma - self.sigma_i
def rela_obsv_update(self, obs_idx, obs_value): ii = 2 * self.index jj = 2 * obs_idx total_sigma = self.sigma_i + self.sigma_d H_ij = matrix( [[0, 0, 0, 0, 0, 0, 0, 0, 0, 0], [0, 0, 0, 0, 0, 0, 0, 0, 0, 0]], dtype=float) H_ij[0, ii] = -1 H_ij[1, ii + 1] = -1 H_ij[0, jj] = 1 H_ij[1, jj + 1] = 1 H = sim_env.rot_mtx(self.theta).getT() * H_ij dis = obs_value[0] phi = obs_value[1] hat_z = H * self.s z = matrix([dis * cos(phi), dis * sin(phi)]).getT() sigma_z = sim_env.rot_mtx(phi) * matrix([[ sim_env.var_dis, 0 ], [0, (dis**2) * sim_env.var_phi]]) * sim_env.rot_mtx(phi).getT() sigma_invention = H * total_sigma * H.getT() + sigma_z kalman_gain = total_sigma * H.getT() * sigma_invention.getI() self.s = self.s + kalman_gain * (z - hat_z) total_sigma = total_sigma - kalman_gain * H * total_sigma self.sigma_i = (i_mtx_10.copy() - kalman_gain * H) * self.sigma_i * ( i_mtx_10.copy() - kalman_gain * H).getT() + kalman_gain * sigma_z * kalman_gain.getT() self.sigma_d = total_sigma - self.sigma_i
def prop_update(self): dt = sim_env.dt # select valid motion input [v, a_v] = [ random.uniform(-sim_env.max_v, sim_env.max_v), random.uniform(-sim_env.max_oemga, sim_env.max_oemga) ] v_star = v + random.normal(0, sqrt(sim_env.var_u_v)) pre_update_position = [ self.position[0] + cos(self.theta) * v_star * dt, self.position[1] + sin(self.theta) * v_star * dt ] while (not sim_env.inRange(pre_update_position, sim_env.origin)): [v, a_v] = [ random.uniform(-sim_env.max_v, sim_env.max_v), random.uniform(-sim_env.max_oemga, sim_env.max_oemga) ] v_star = v + random.normal(0, sqrt(sim_env.var_u_v)) pre_update_position = [ self.position[0] + cos(self.theta) * v_star * dt, self.position[1] + sin(self.theta) * v_star * dt ] # real position update self.position[0] += cos(self.theta) * v_star * dt self.position[1] += sin(self.theta) * v_star * dt self.theta += a_v * dt # estimation update ii = 2 * self.index self.s[ii, 0] = self.s[ii, 0] + cos(self.theta) * v * dt self.s[ii + 1, 0] = self.s[ii + 1, 0] + sin(self.theta) * v * dt # covariance update for j in range(RobotLocalCI.N): jj = 2 * j if j == self.index: self.sigma[jj:jj + 2, jj:jj + 2] += dt * dt * sim_env.rot_mtx( self.theta) * matrix([ [sim_env.var_u_v, 0], [0, sim_env.var_u_theta] ]) * sim_env.rot_mtx(self.theta).getT() self.th_sigma[jj:jj + 2, jj:jj + 2] += dt * dt * sim_env.var_u_v * matrix( [[1, 0], [0, 1]]) else: self.sigma[ jj:jj + 2, jj:jj + 2] += dt * dt * sim_env.var_v * sim_env.i_mtx_2.copy() self.th_sigma[ jj:jj + 2, jj:jj + 2] += dt * dt * sim_env.var_v * sim_env.i_mtx_2.copy()
def rela_obsv(self, idx, obs_idx, obs_value): i = 2 * idx j = 2 * obs_idx H_ij = matrix( [[0, 0, 0, 0, 0, 0, 0, 0, 0, 0], [0, 0, 0, 0, 0, 0, 0, 0, 0, 0]], dtype=float) H_ij[0, i] = -1 H_ij[1, i + 1] = -1 H_ij[0, j] = 1 H_ij[1, j + 1] = 1 H = sim_env.rot_mtx(self.theta[idx]).getT() * H_ij dis = obs_value[0] phi = obs_value[1] z = matrix([[dis * cos(phi)], [dis * sin(phi)]]) hat_j = z + self.s[i:i + 2] sigma_i = self.sigma[i:i + 2, i:i + 2] sigma_j = self.sigma[j:j + 2, j:j + 2] e = 0.83 sigma_j_next_inv = e * sigma_j.getI() + (1 - e) * sigma_i.getI() self.s[j:j + 2] = sigma_j_next_inv.getI() * ( e * sigma_j.getI() * self.s[j:j + 2] + (1 - e) * sigma_i.getI() * hat_j) self.sigma[j:j + 2, j:j + 2] = sigma_j_next_inv.getI()
def rela_obsv_update(self, idx, obs_idx, obs_value): ii = 2 * idx jj = 2 * obs_idx ci_coeff = 0.83 sigma_j = (1 / ci_coeff) * self.sigma_d[jj:jj + 2, jj:jj + 2] + self.sigma_i[jj:jj + 2, jj:jj + 2] sigma_i = (1 / (1 - ci_coeff)) * self.sigma_d[ii:ii + 2, ii:ii + 2] + self.sigma_i[ii:ii + 2, ii:ii + 2] dis = obs_value[0] phi = obs_value[1] z = matrix([[dis * cos(phi)], [dis * sin(phi)]]) hat_j = self.s[ii:ii + 2] + sim_env.rot_mtx(self.theta[idx]) * z kalman_gain = sigma_j * (sigma_i + sigma_j).getI() self.s[jj:jj + 2] = self.s[jj:jj + 2] + kalman_gain * (hat_j - self.s[jj:jj + 2]) total_sigma = (i_mtx_2 - kalman_gain) * sigma_j self.sigma_i[jj:jj + 2, jj:jj + 2] = ( i_mtx_2 - kalman_gain) * self.sigma_i[jj:jj + 2, jj:jj + 2] * ( i_mtx_2 - kalman_gain).getT() + kalman_gain * self.sigma_i[ ii:ii + 2, ii:ii + 2] * kalman_gain.getT() self.sigma_d[jj:jj + 2, jj:jj + 2] = total_sigma - self.sigma_i[jj:jj + 2, jj:jj + 2]
def motion_propagation_update(self, odometry_input, dt): [v, omega] = odometry_input ii = 2 * self.index # estimation update self.s[ii, 0] = self.s[ii, 0] + cos(self.theta) * v * dt self.s[ii + 1, 0] = self.s[ii + 1, 0] + sin(self.theta) * v * dt # covariance update for j in range(sim_env.N): jj = 2 * j if j == self.index: rot_mtx_theta = sim_env.rot_mtx(self.theta) self.sigma_i[jj:jj + 2, jj:jj + 2] += dt * dt * rot_mtx_theta * matrix([[ sim_env.var_u_v, 0 ], [0, 0]]) * rot_mtx_theta.T else: self.sigma_i[ jj:jj + 2, jj:jj + 2] += dt * dt * sim_env.var_v * sim_env.i_mtx_2.copy()
def prop_update(self): for idx in range(5): dt = sim_env.dt ii = 2*idx # select valid motion input [v, a_v] = [random.uniform(-sim_env.max_v, sim_env.max_v), random.uniform(-sim_env.max_oemga, sim_env.max_oemga)] v_star = v + random.normal(0, sqrt(sim_env.var_u_v)) pre_update_position = [self.position[ii] + cos(self.theta[idx])*v_star*dt, self.position[ii+1] + sin(self.theta[idx])*v_star*dt] while(not sim_env.inRange(pre_update_position, sim_env.origin)): [v, a_v] = [random.uniform(-sim_env.max_v, sim_env.max_v), random.uniform(-sim_env.max_oemga, sim_env.max_oemga)] v_star = v + random.normal(0, sqrt(sim_env.var_u_v)) pre_update_position = [self.position[ii] + cos(self.theta[idx])*v_star*dt, self.position[ii+1] + sin(self.theta[idx])*v_star*dt] # real position update self.position[ii,0] += cos(self.theta[idx])*v_star*dt self.position[ii+1,0] += sin(self.theta[idx])*v_star*dt self.theta[idx] += a_v*dt # estimation update self.s[ii,0] += cos(self.theta[idx])*v*dt self.s[ii+1,0] += sin(self.theta[idx])*v*dt # covariance update self.sigma[ii:ii+2, ii:ii+2] += dt*dt*sim_env.rot_mtx(self.theta[idx])*matrix([[sim_env.var_u_v, 0],[0, sim_env.var_u_theta]])*sim_env.rot_mtx(self.theta[idx]).getT() self.th_sigma[ii:ii+2, ii:ii+2] += dt*dt*sim_env.var_u_v*matrix([[1, 0],[0, 1]])
def ablt_obsv(self, idx, obs_value, landmark): ii = 2 * idx H = sim_env.rot_mtx(self.theta[idx]).getT() * matrix( [[-1, 0], [0, -1]], dtype=float) local_s = self.s[ii:ii + 2] local_sigma = self.sigma[ii:ii + 2, ii:ii + 2] dis = obs_value[0] phi = obs_value[1] hat_z = sim_env.rot_mtx( self.theta[idx]).getT() * (landmark.position + H * local_s) z = matrix([dis * cos(phi), dis * sin(phi)]).getT() sigma_z = sim_env.rot_mtx(phi) * matrix([[ sim_env.var_dis, 0 ], [0, dis * dis * sim_env.var_phi]]) * sim_env.rot_mtx(phi).getT() sigma_invention = H * local_sigma * H.getT() + sigma_z kalman_gain = local_sigma * H.getT() * sigma_invention.getI() self.s[ii:ii + 2] = local_s + kalman_gain * (z - hat_z) self.sigma[ii:ii + 2, ii:ii + 2] = local_sigma - kalman_gain * H * local_sigma multi_i = (sim_env.i_mtx_2.copy() - kalman_gain * H) for k in range(5): if (not (k == idx)): kk = 2 * k self.sigma[ii:ii + 2, kk:kk + 2] = multi_i * self.sigma[ii:ii + 2, kk:kk + 2] self.sigma[kk:kk + 2, ii:ii + 2] = self.sigma[kk:kk + 2, ii:ii + 2] * multi_i.getT()
def motion_propagation_update(self, odometry_input, dt): for i in range(sim_env.N): [v, omega] = odometry_input[i] ii = 2*i self.theta[i] = self.theta[i] + omega * dt # estimation update self.s[ii,0] += cos(self.theta[i]) * v * dt self.s[ii+1,0] += sin(self.theta[i]) * v * dt # covariance update rot_mtx_theta_i = sim_env.rot_mtx(self.theta[i]) self.sigma[ii:ii+2, ii:ii+2] += dt*dt*rot_mtx_theta_i*matrix([[sim_env.var_u_v, 0],[0, 0]])*rot_mtx_theta_i.T