I am new in localization algorithms and EKF is new for me as well. I think that I implemented EKF (the whole code of class is here. I have problems in updating bearing angle of the robot. Basically I have to subtract expected measurement from real measurement. Or $$ [range_{measured}, bearing_{measured}] - [range_{expected}, bearing_{expected}]$$. I am stuck a bit with that stuff. Please give me some advice on how to get correct bearing for innovation step.

Here is my measurement model function:

def measurement_model(self,state):
        x = state[0]
        y = state[1]
        theta = state[2]
        px = self.cur_id[0] # x coordinate of current beacon
        py = self.cur_id[1] # y coordinate of current beacon

        r = np.sqrt((px-x)**2 + (py-y)**2)      #Distance
        phi = np.arctan2(py-y, px-x) - theta    #Bearing

        self.Z[0] = r
        self.Z[1] = phi 
        return self.Z

Update function:

def update(self, msg): #
        self.cur_id = self.beacons[msg.ids[0]] # coordinates of current transmitter

        # landmark position in robot frame
        pos_x = msg.pose.position.x
        pos_y = msg.pose.position.y
        rng = np.sqrt(pos_x**2 + pos_y**2)
        theta = self.wrap_to_pi(euler_from_quaternion([msg.pose.orientation.x, msg.pose.orientation.y, msg.pose.orientation.z, msg.pose.orientation.w])[2])
        new_theta_meas = np.arctan2(self.cur_id[1] - pos_y, self.cur_id[0] - pos_x) - theta # HOW DO I GET THIS CORRECTLY

        floor = self.cov_matrix.dot(self.obs_j_state.transpose()).astype(np.float32)

        bottom = (self.obs_j_state.dot(self.cov_matrix).dot(self.obs_j_state.transpose()) + np.eye(2)*0.01).astype(np.float32)

        self.K = floor.dot(np.linalg.inv(bottom))

        expected_meas = self.measurement_model(self.state_vector)
        tempterm = np.array(([rng - expected_meas[0],new_theta_meas - expected_meas[1]]))

        self.state_vector = self.state_vector + self.K.dot(tempterm)
        self.cov_matrix = (np.eye(3) - self.K.dot(self.obs_j_state)).dot(self.cov_matrix)
  • $\begingroup$ Would you mind narrowing down the question? This is quite broad. $\endgroup$
    – holmeski
    Feb 27, 2020 at 0:56
  • $\begingroup$ Ok. I want to know how to make innovation in case of bearing angle. The bearing angle from the beacon is in robot's frame, in the state vector bearing on the map frame $\endgroup$ Feb 27, 2020 at 1:09
  • $\begingroup$ Do you know where the feature is located in the world frame or are you estimating it? $\endgroup$
    – holmeski
    Feb 27, 2020 at 2:36
  • $\begingroup$ Yes, ai know where landmarks are by their coordinates in world frame. $\endgroup$ Feb 27, 2020 at 7:12
  • $\begingroup$ I meant, the coordinates if the beacons are given. Sorry fir inconsistent way of asking and providing information $\endgroup$ Feb 27, 2020 at 7:45


Your Answer

By clicking “Post Your Answer”, you agree to our terms of service and acknowledge you have read our privacy policy.

Browse other questions tagged or ask your own question.