Question

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

Answer 1

# ----------
# Background
# 
# A robotics company named Trax has created a line of small self-driving robots 
# designed to autonomously traverse desert environments in search of undiscovered
# water deposits.
#
# A Traxbot looks like a small tank. Each one is about half a meter long and drives
# on two continuous metal tracks. In order to maneuver itself, a Traxbot can do one
# of two things: it can drive in a straight line or it can turn. So to make a 
# right turn, A Traxbot will drive forward, stop, turn 90 degrees, then continue
# driving straight.
#
# This series of questions involves the recovery of a rogue Traxbot. This bot has 
# gotten lost somewhere in the desert and is now stuck driving in an almost-circle: it has
# been repeatedly driving forward by some step size, stopping, turning a certain 
# amount, and repeating this process... Luckily, the Traxbot is still sending all
# of its sensor data back to headquarters.
#
# In this project, we will start with a simple version of this problem and 
# gradually add complexity. By the end, you will have a fully articulated
# plan for recovering the lost Traxbot.
# 
# ----------
# Part One
#
# Let's start by thinking about circular motion (well, really it's polygon motion
# that is close to circular motion). Assume that Traxbot lives on 
# an (x, y) coordinate plane and (for now) is sending you PERFECTLY ACCURATE sensor 
# measurements. 
#
# With a few measurements you should be able to figure out the step size and the 
# turning angle that Traxbot is moving with.
# With these two pieces of information, you should be able to 
# write a function that can predict Traxbot's next location.
#
# You can use the robot class that is already written to make your life easier. 
# You should re-familiarize yourself with this class, since some of the details
# have changed. 
#
# ----------
# YOUR JOB
#
# Complete the estimate_next_pos function. You will probably want to use
# the OTHER variable to keep track of information about the runaway robot.
#
# ----------
# 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 *
from math import *
from matrix import *
import random

    
def estimate_next_pos(measurement, OTHER = None):
    xy_estimate = 0, 0 #something for initial returns
    if OTHER is None:
        OTHER = [measurement]
        #turning_angleSum = 0.
    elif len(OTHER) < 3:
        OTHER.append(measurement)
    elif len(OTHER) >= 3:
        OTHER.append(measurement)
        turning_angleSum = 0.
        distanceSum = 0.
        #use mean of turning angles so far
        for i in range( len(OTHER)-2):
            point0 = OTHER[i]
            point1 = OTHER[i + 1]
            point2 = OTHER[i + 2]
    
            dx0 = point1[0] - point0[0]
            dy0 = point1[1] - point0[1]
            dist0 = distance_between(point0, point1) 

            dx1 = point2[0] - point1[0]
            dy1 = point2[1] - point1[1]
            distance1 = distance_between(point1, point2)
            distanceSum += dist0#ance_between(point0, point1)
                
            dot = dx1*dx0 + dy1*dy0
            turning_angleSum += acos(dot/(dist0 * distance1))
        distanceSum += distance_between(OTHER[-1], OTHER[-2])
        #print '1st=',distanceSum
        distance = distanceSum/(len(OTHER) - 1)
        print distance
        turning_angle = turning_angleSum/(len(OTHER)-2)

        #use previous two points to compute
        point2 = OTHER[-1]
        point1 = OTHER[-2]
        
        heading = atan2(dy1,dx1)
        heading = angle_trunc(heading)
        myRobot = robot(measurement[0], measurement[1], heading, turning_angle, distance)
        myRobot.set_noise(0.01, 0.01, 0)
        myRobot.move_in_circle()        

        xy_estimate = myRobot.x, myRobot.y
                           

    return xy_estimate, OTHER 

# 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)

import statistics

def reject_outliers(data):
    m = 2
    u = statistics.mean(data)
    s = statistics.stdev(data)
    filtered = [e for e in data if (u - 2 * s < e < u + 2 * s)]
    return filtered
# 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.
from demo_grading import * 
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 <= 200: 
        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)
        #print 'dist=',error#, 'guess=',position_guess, 'true=',true_position
        if error <= distance_tolerance:
            print "You got it right! It took you ", ctr, " steps to localize."
            localized = True
        if ctr == 200:
            print "Sorry, it took you too many steps to localize the target."
    return localized



# 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., 1.5)
measurement_noise = 0.05 * test_target.distance
test_target.set_noise(0.0, 0.0, measurement_noise)
print 'test_target = ',test_target

demo_grading(estimate_next_pos, test_target, OTHER = None)
#print demo_grading(hunter, test_target, next_move)