EKF landmark-based help needed

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
y = state
theta = state
px = self.cur_id # x coordinate of current beacon
py = self.cur_id # 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 = r
self.Z = phi
return self.Z

Update function:

def update(self, msg): #
self.cur_id = self.beacons[msg.ids] # 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]))
self.observation_jacobian_state_vector()
new_theta_meas = np.arctan2(self.cur_id - pos_y, self.cur_id - 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,new_theta_meas - expected_meas]))

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)
• Would you mind narrowing down the question? This is quite broad. Feb 27 '20 at 0:56
• 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 Feb 27 '20 at 1:09
• Do you know where the feature is located in the world frame or are you estimating it? Feb 27 '20 at 2:36
• Yes, ai know where landmarks are by their coordinates in world frame. Feb 27 '20 at 7:12
• I meant, the coordinates if the beacons are given. Sorry fir inconsistent way of asking and providing information Feb 27 '20 at 7:45