I am programing an Extended Kalman Filter , with noise but not getting correct answer .

this is my code

# ---------- # Part Two # # Now we'll make the scenario a bit more realistic. Now Traxbot's # sensor measurements are a bit noisy (though its motions are still # completetly noise-free and it still moves in an almost-circle). # You'll have to write a function that takes as input the next # noisy (x, y) sensor measurement and outputs the best guess # for the robot's next position. # # ---------- # YOUR JOB # # Complete the function estimate_next_pos. You will be considered # correct if your estimate is within 0.01 stepsizes of Traxbot's next # true position. # # ---------- # GRADING # # We will make repeated calls to your estimate_next_pos function. After # each call, we will compare your estimated position to the robot's true # position. As soon as you are within 0.01 stepsizes of the true position, # you will be marked correct and we will tell you how many steps it took # before your function successfully located the target bot.

# These import steps give you access to libraries which you may (or may # not) want to use. from robot import * # Check the robot.py tab to see how this works. from math import * from matrix import * # Check the matrix.py tab to see how this works. import random

# This is the function you have to write. Note that measurement is a # single (x, y) point. This function will have to be called multiple # times before you have enough information to accurately predict the # next position. The OTHER variable that your function returns will be # passed back to your function the next time it is called. You can use # this to keep track of important information over time.

# A helper function you may find useful. def distance_between(point1, point2): """Computes distance between point1 and point2. Points are (x, y) pairs.""" x1, y1 = point1 x2, y2 = point2 return sqrt((x2 - x1) ** 2 + (y2 - y1) ** 2) def estimate_next_pos(measurement, OTHER = None): """Estimate the next (x, y) position of the wandering Traxbot based on noisy (x, y) measurements."""

#print "meas:", measurement x1 = measurement[0] y1 = measurement[1]

if not OTHER: OTHER = [[],[],[],[]] # inital guesses: import numpy as np x0 = np.random.normal(0, 0.1, 1) y0 = np.random.normal(0, 0.1, 1) dist0 = np.random.normal(0, 0.1, 1) theta0 =np.random.normal(pi/2, pi/5, 1) dtheta0 = np.random.normal(pi/2, pi/5, 1) # initial uncertainty: P = matrix([[100.,0.,0.], [0.,100.,0.], [0.,0.,100.]]) else: # pull previous measurement, state variables (x), and uncertainty (P) from OTHER lastMeasurement = OTHER[0][len(OTHER[0])-1] x0 = lastMeasurement[0] y0 = lastMeasurement[1] dist0 = OTHER[1].value[0][0] theta0 = OTHER[1].value[1][0] % (2*pi) dtheta0 = OTHER[1].value[2][0] P = OTHER[2]

# convert measurement to dist and theta (based on previous estimate) dist = distance_between([x1,y1],[x0,y0]) theta = atan2(y1-y0,x1-x0) # time step dt = 1. # state matrix (polar location and angular velocity) x = matrix([[dist0], [theta0], [dtheta0]]) # external motion u = matrix([[0.], [0.], [0.]]) # next state function: F = matrix([[1.,0.,0.], [0.,1.,dt], # theta is the only thing that should change, by dtheta [0.,0.,1.]]) # measurement function: H = matrix([[1.,0.,0.], [0.,1.,0.]]) # measurement uncertainty: import numpy as np s = np.random.normal(0.5, 0.1, 1) R = matrix([[s,0.], [0.,s]]) # 5d identity matrix I = matrix([[]]) I.identity(3)

OTHER[3].append([dist,theta]) res=robot() distance=0 #res.distance_noise=random.gauss(lastMeasurement[0],self.distance_noise) import numpy as np mu, sigma = 0.05, 0.01 s = np.random.normal(mu, sigma, 1) p=np.random.normal(pi,pi/5, 1) t=np.random.normal(0.5, 0.1, 1) Q = matrix([[s,0.,0.], [0.,s,0.], [0.,0.,s]]) #for i in range(len(OTHER[3])): # prediction x = (F * x) + u P = (F * P * F.transpose())+Q # measurement update #Z = matrix([[OTHER[3][i][0],OTHER[3][i][1]]]) Z = matrix([[dist,theta]]) y = Z.transpose() - (H * x) S = H * P * H.transpose() + R K = P * H.transpose() * S.inverse() x = x + (K * y) P = (I - (K * H)) * P OTHER[0].append(measurement) OTHER[1] = x OTHER[2] = P #print "x:" #x.show() #print "P:" #P.show() xy_estimate = (x1+x.value[0][0]*cos((x.value[1][0]+x.value[2][0])%(2*pi)), y1+x.value[0][0]*sin((x.value[1][0]+x.value[2][0])%(2*pi))) #xy_estimate = (x1+x.value[0][0]*cos((x.value[1][0])), # y1+x.value[0][0]*sin((x.value[1][0]))) #print xy_estimate # You must return xy_estimate (x, y), and OTHER (even if it is None) # in this order for grading purposes. return xy_estimate, OTHER

# This is here to give you a sense for how we will be running and grading # your code. Note that the OTHER variable allows you to store any # information that you want. def demo_grading(estimate_next_pos_fcn, target_bot, OTHER = None): localized = False distance_tolerance = 0.01 * target_bot.distance ctr = 0 # if you haven't localized the target bot, make a guess about the next # position, then we move the bot and compare your guess to the true # next position. When you are close enough, we stop checking. while not localized and ctr <= 1000: ctr += 1 measurement = target_bot.sense() position_guess, OTHER = estimate_next_pos_fcn(measurement, OTHER) target_bot.move_in_circle() true_position = (target_bot.x, target_bot.y) error = distance_between(position_guess, true_position) if error <= distance_tolerance: print "You got it right! It took you ", ctr, " steps to localize." localized = True if ctr == 1000: print "Sorry, it took you too many steps to localize the target." return localized

# This is a demo for what a strategy could look like. This one isn't very good. def naive_next_pos(measurement, OTHER = None): """This strategy records the first reported position of the target and assumes that eventually the target bot will eventually return to that position, so it always guesses that the first position will be the next.""" if not OTHER: # this is the first measurement OTHER = measurement xy_estimate = OTHER return xy_estimate, OTHER

# This is how we create a target bot. Check the robot.py file to understand # How the robot class behaves. test_target = robot(2.1, 4.3, 0.5, 2*pi / 34.0, 1.5) measurement_noise = 0.05 * test_target.distance test_target.set_noise(0.0, 0.0, measurement_noise)

demo_grading(naive_next_pos, test_target)

I have only implemented the function - estimate_next_pos