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$
  • $\begingroup$ Would you mind narrowing down the question? This is quite broad. $\endgroup$ – holmeski Feb 27 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$ – Hillbilly Joe Feb 27 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 at 2:36
  • $\begingroup$ Yes, ai know where landmarks are by their coordinates in world frame. $\endgroup$ – Hillbilly Joe Feb 27 at 7:12
  • $\begingroup$ I meant, the coordinates if the beacons are given. Sorry fir inconsistent way of asking and providing information $\endgroup$ – Hillbilly Joe Feb 27 at 7:45

Your Answer

By clicking “Post Your Answer”, you agree to our terms of service, privacy policy and cookie policy

Browse other questions tagged or ask your own question.