0
$\begingroup$

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)
        #bearing
        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])
        self.observation_jacobian_state_vector()
        new_theta_meas = np.arctan2(self.cur_id[1] - pos_y, self.cur_id[0] - pos_x) - theta # HOW DO I GET THIS CORRECTLY

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

        #denominator
        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)
        print(self.state_vector)
$\endgroup$
7
  • $\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

0

Your Answer

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

Browse other questions tagged or ask your own question.