课程2:Problem Set 1

课程2:Problem Set 1

2. 练习:Localization

平面环境中机器人的一般有3个坐标轴: (x,y,orientation) orientation 是面向的角度。

而立体空间中,飞行机器人,一般有6个坐标轴:(x,y,z, roll,pitch,yaw)

状态变量从3变化为6,使用基于直方图的定位,对于内存使用量来说,是指数变化的?

3. 练习:Bayes’ Rule

P(F)=0.001 失火的概率

B = 邻居说“失火了”

P(邻居撒谎的概率)=0.1

当邻居说“失火了”时,

真正起火的概率为 P1(F|B) = 0.0009

没有起火的概率为 P1(~F|B) = 0.0999

归一化后,P(F|B)=0.0089

                 P(~F|B)=0.991

4. 练习:Localization Program

二维移动的例子,代码如下:

# -*- coding: utf-8 -*-

# The function localize takes the following arguments:

#

# colors:

#        2D list, each entryeither 'R' (for red cell) or 'G' (for green cell)

#

# measurements:

#        list of measurementstaken by the robot, each entry either 'R' or 'G'

#

# motions:

#        list of actions takenby the robot, each entry of the form [dy,dx],

#        where dx refers tothe change in the x-direction (positive meaning

#        movement to theright) and dy refers to the change in the y-direction

#        (positive meaningmovement downward)

#        NOTE: the *first*coordinate is change in y; the *second* coordinate is

#              change in x

#

# sensor_right:

#        float between 0 and1, giving the probability that any given

#        measurement iscorrect; the probability that the measurement is

#        incorrect is1-sensor_right

#

# p_move:

#        float between 0 and1, giving the probability that any given movement

#        command takes place;the probability that the movement command fails

#        (and the robotremains still) is 1-p_move; the robot will NOT overshoot

#        its destination inthis exercise

#

# The function should RETURN (not just show or print) a 2D list (ofthe same

# dimensions as colors) that gives the probabilities that the robotoccupies

# each cell in the world.

#

# Compute the probabilities by assuming the robot initially has auniform

# probability of being in any cell.

#

# Also assume that at each step, the robot:

# 1) first makes a movement,

# 2) then takes a measurement.

#

# Motion:

#  [0,0] - stay

#  [0,1] - right

#  [0,-1] - left

#  [1,0] - down

#  [-1,0] - up

 

def localize(colors,measurements,motions,sensor_right,p_move):

    # initializes p to auniform distribution over a grid of the same dimensions as colors

    pinit = 1.0 / float(len(colors))/ float(len(colors[0]))

    p = [[pinit for row inrange(len(colors[0]))] for col in range(len(colors))]

   

    # >>> Insert yourcode here <<<

    if len(motions) !=len(measurements):

        print("localize()meet a exception ! the size of motions and measurements should be equal.")

        raise

    #show(p)

    for m inrange(len(motions)):

        print('----enter forloop index {} move is [{}][{}]'.format(m,motions[m][0], motions[m][1]))

        p = move(p,motions[m], p_move)

       print('-------------------------begin to sense()')

        p = sense(p, colors,measurements[m], sensor_right)

        #show(p) # for debug

               

    return p

 

def move(p, movements, p_move):

    print('move() invoked')

    q = [[0 for col inrange(len(p[0]))] for row in range(len(p))]

    newMove = [0,0]

    newMove[0] = movements[0]% len(p)

    newMove[1] = movements[1]% len(p[0])

 

    for r in range(len(p)) :

            for c inrange(len(p[r])) :

                # not move,just copy the p list

                if newMove[0]== 0 and newMove[1] == 0:

                    q[r][c] =p[r][c]

                    continue

                # move thefirst dimention, when move, do not change dimen

                mx = r

                if newMove[0]> 0 : # down

                    mx =(r-newMove[0])%len(p)

                   print("[{0}][{1}] move down\n".format(r,c))

                elifnewMove[0] < 0 : # up

                    mx =(r+newMove[0])%len(p)

                   print("[{0}][{1}] move up\n".format(r,c))

               

                # move thesecond dimention, when move, do not change dimen

                my = c

                if newMove[1]> 0 : # right

                    my =(c-newMove[1])%len(p[r])

                   print("[{0}][{1}] move right\n".format(r,c))

                elifnewMove[1] < 0 : # left

                    my =(c+newMove[1])%len(p[r])

                   print("[{0}][{1}] move left\n".format(r,c))

                   

                q[r][c] = p[mx][my] * p_move + p[r][c] *(1-p_move)

 

                #if q[r][c] !=0:

                #    print('qv = {}, qh = {}'.format(qv, qh))

    #show(p)

    #print('p---move to----q')

    #show(q)

 

    total = 0

    for r in range(len(q)) :

        total += sum(q[r])

   

    for r in range(len(q)) :

            for c inrange(len(q[r])) :

                q[r][c] =q[r][c] / total

                       

    return q

 

def sense(p, colors, measureVal, sensor_right):

    print('sense() invoked')

    q = [[0 for col inrange(len(p[0]))] for row in range(len(p))]

    for r in range(len(p)) :

        for c inrange(len(p[r])) :

            # sense

            if measureVal ==colors[r][c] :

                qrc = p[r][c]* sensor_right

               print("sense [{0}][{1}] measure {2}\n".format(r,c,measureVal))

            else :

                qrc = p[r][c]* (1 - sensor_right)

            q[r][c] = qrc

 

    total = 0

    for r in range(len(q)) :

        total += sum(q[r])

   

    for r in range(len(q)) :

        for c inrange(len(q[r])) :

            q[r][c] = q[r][c]/ total

    return q

 

def show(p):

    rows = ['[' +','.join(map(lambda x: '{0:.5f}'.format(x),r)) + ']' for r in p]

    print('[' + ',\n'.join(rows) + ']')

   

#############################################################

# For the following test case, your output should be

# [[0.01105, 0.02464, 0.06799, 0.04472, 0.02465],

#  [0.00715, 0.01017, 0.08696,0.07988, 0.00935],

#  [0.00739, 0.00894, 0.11272,0.35350, 0.04065],

#  [0.00910, 0.00715, 0.01434,0.04313, 0.03642]]

# (within a tolerance of +/- 0.001 for each entry)

 

colors = [['R','G','G','R','R'],

         ['R','R','G','R','R'],

         ['R','R','G','G','R'],

          ['R','R','R','R','R']]

measurements = ['G','G','G','G','G']

motions = [[0,0],[0,1],[1,0],[1,0],[0,1]]

p = localize(colors,measurements,motions,sensor_right = 0.7, p_move= 0.8)

show(p) # displays your answer

# the result is :

# [[0.01106,0.02464,0.06800,0.04472,0.02465],

# [0.00715,0.01017,0.08697,0.07988,0.00935],

# [0.00740,0.00894,0.11273,0.35351,0.04066],

# [0.00911,0.00715,0.01435,0.04313,0.03643]]

#

#

#

 

# test 1

#colors = [['G', 'G', 'G'],

#          ['G', 'R', 'G'],

#          ['G', 'G', 'G']]

#measurements = ['R']

#motions = [[0,0]]

#sensor_right = 1.0

#p_move = 1.0

#p = localize(colors,measurements,motions,sensor_right,p_move)

#show(p) # test pass

    

# test 2

#colors = [['G', 'G', 'G'],

#          ['G', 'R', 'R'],

#          ['G', 'G', 'G']]

#measurements = ['R']

#motions = [[0,0]]

#sensor_right = 1.0

#p_move = 1.0

#p = localize(colors,measurements,motions,sensor_right,p_move)

#show(p) # test pass

 

# test 3

#colors = [['G', 'G', 'G'],

#          ['G', 'R', 'R'],

#          ['G', 'G', 'G']]

#measurements = ['R']

#motions = [[0,0]]

#sensor_right = 0.8

#p_move = 1.0

#p = localize(colors,measurements,motions,sensor_right,p_move)

#show(p) # test pass

 

# test 4

#colors = [['G', 'G', 'G'],

#          ['G', 'R', 'R'],

#          ['G', 'G', 'G']]

#measurements = ['R', 'R']

#motions = [[0,0], [0,1]]

#sensor_right = 0.8

#p_move = 1.0

#p = localize(colors,measurements,motions,sensor_right,p_move)

#show(p) # test pass

 

# test 5

#colors = [['G', 'G', 'G'],

#          ['G', 'R', 'R'],

#          ['G', 'G', 'G']]

#measurements = ['R', 'R']

#motions = [[0,0], [0,1]]

#sensor_right = 1.0

#p_move = 1.0

#p = localize(colors,measurements,motions,sensor_right,p_move)

#show(p) # test pass

 

# test 6

#colors = [['G', 'G', 'G'],

#          ['G', 'R', 'R'],

#          ['G', 'G', 'G']]

#measurements = ['R', 'R']

#motions = [[0,0], [0,1]]

#sensor_right = 0.8

#p_move = 0.5

#p = localize(colors,measurements,motions,sensor_right,p_move)

#show(p) # test pass

 

# test 7

#colors = [['G', 'G', 'G'],

#          ['G', 'R', 'R'],

#          ['G', 'G', 'G']]

#measurements = ['R', 'R']

#motions = [[0,0], [0,1]]

#sensor_right = 1.0

#p_move = 0.5

#p = localize(colors,measurements,motions,sensor_right,p_move)

#show(p) # test pass

其实,move()中应该可以不用归一化,因为sense()中每次会进行归一化,而且sense()是每次最后调用的方法。

你可能感兴趣的:(课程笔记)