コード例 #1
0
                   [12.849049222879723, -15.326510824972983]],
                  [[[0, 10.591050242096598, -39.2051798967113],
                    [1, -3.5675572049297553, 22.849456408289125],
                    [2, -38.39251065320351, 7.288990306029511]],
                   [12.849049222879723, -15.326510824972983]],
                  [[[0, -3.6225556479370766, -25.58006865235512]],
                   [-7.8874682868419965, -18.379005523261092]],
                  [[[0, 1.9784503557879374, -6.5025974151499]],
                   [-7.8874682868419965, -18.379005523261092]],
                  [[[0, 10.050665232782423, 11.026385307998742]],
                   [-17.82919359778298, 9.062000642947142]],
                  [[[0, 26.526838150174818, -0.22563393232425621],
                    [4, -33.70303936886652, 2.880339841013677]],
                   [-17.82919359778298, 9.062000642947142]]]
    result = slam(test_data1, 20, 5, world_size, 2.0, 2.0)
    print_result(20, 5, result)
    check_mu(result, answer_mu)

    ##  Test Case 2
    answer_mu = matrix([
        ##  Estimated Pose(s):
        [49.999477332348086],
        [49.99890156551778],
        [69.18030735650325],
        [45.66363344648087],
        [87.7418064828167],
        [39.7015808043011],
        [76.2689002984748],
        [56.308708837988604],
        [64.31595832940498],
        [72.17416433180664],
     [87.12997140410576 ], [38.53576961176431 ],
     [80.3007799395094  ], [20.505859780712917],
     [72.79656231764604 ], [2.942675607797428 ],
     [55.243590482706026], [13.253021809459227],
     [37.41439688492312 ], [22.31502245240636 ],
     
     ##  Estimated Landmarks:
     [82.95425645256424 ], [13.536536707427121],
     [70.49306062345174 ], [74.13913606764582 ],
     [36.73812342335858 ], [61.2789905549488  ],
     [18.696326102039485], [66.05733561281015 ],
     [20.632945056999347], [16.87255837889543]
 ])
 test_data1 = [[[[1, 19.457599255548065, 23.8387362100849], [2, -13.195807561967236, 11.708840328458608], [3, -30.0954905279171, 15.387879242505843]], [-12.2607279422326, -15.801093326936487]], [[[2, -0.4659930049620491, 28.088559771215664], [4, -17.866382374890936, -16.384904503932]], [-12.2607279422326, -15.801093326936487]], [[[4, -6.202512900833806, -1.823403210274639]], [-12.2607279422326, -15.801093326936487]], [[[4, 7.412136480918645, 15.388585962142429]], [14.008259661173426, 14.274756084260822]], [[[4, -7.526138813444998, -0.4563942429717849]], [14.008259661173426, 14.274756084260822]], [[[2, -6.299793150150058, 29.047830407717623], [4, -21.93551130411791, -13.21956810989039]], [14.008259661173426, 14.274756084260822]], [[[1, 15.796300959032276, 30.65769689694247], [2, -18.64370821983482, 17.380022987031367]], [14.008259661173426, 14.274756084260822]], [[[1, 0.40311325410337906, 14.169429532679855], [2, -35.069349468466235, 2.4945558982439957]], [14.008259661173426, 14.274756084260822]], [[[1, -16.71340983241936, -2.777000269543834]], [-11.006096015782283, 16.699276945166858]], [[[1, -3.611096830835776, -17.954019226763958]], [-19.693482634035977, 3.488085684573048]], [[[1, 18.398273354362416, -22.705102332550947]], [-19.693482634035977, 3.488085684573048]], [[[2, 2.789312482883833, -39.73720193121324]], [12.849049222879723, -15.326510824972983]], [[[1, 21.26897046581808, -10.121029799040915], [2, -11.917698965880655, -23.17711662602097], [3, -31.81167947898398, -16.7985673023331]], [12.849049222879723, -15.326510824972983]], [[[1, 10.48157743234859, 5.692957082575485], [2, -22.31488473554935, -5.389184118551409], [3, -40.81803984305378, -2.4703329790238118]], [12.849049222879723, -15.326510824972983]], [[[0, 10.591050242096598, -39.2051798967113], [1, -3.5675572049297553, 22.849456408289125], [2, -38.39251065320351, 7.288990306029511]], [12.849049222879723, -15.326510824972983]], [[[0, -3.6225556479370766, -25.58006865235512]], [-7.8874682868419965, -18.379005523261092]], [[[0, 1.9784503557879374, -6.5025974151499]], [-7.8874682868419965, -18.379005523261092]], [[[0, 10.050665232782423, 11.026385307998742]], [-17.82919359778298, 9.062000642947142]], [[[0, 26.526838150174818, -0.22563393232425621], [4, -33.70303936886652, 2.880339841013677]], [-17.82919359778298, 9.062000642947142]]]
 result = slam(test_data1, 20, 5, world_size, 2.0, 2.0)
 print_result(20, 5, result)
 check_mu(result, answer_mu)
 
 ##  Test Case 2
 answer_mu = matrix([
     ##  Estimated Pose(s):
     [49.999477332348086], [49.99890156551778 ],
     [69.18030735650325 ], [45.66363344648087 ],
     [87.7418064828167  ], [39.7015808043011  ],
     [76.2689002984748  ], [56.308708837988604],
     [64.31595832940498 ], [72.17416433180664 ],
     [52.25593780877982 ], [88.15129488583568 ],
     [44.05788183012242 ], [69.39888197470297 ],
     [37.00060088117602 ], [49.91574097036883 ],
     [30.923168865968172], [30.953132613521404],
     [23.507016577616362], [11.417418859810084],
コード例 #3
0
#
# Main routine
#
if __name__ == '__main__':
    num_landmarks      = 5        # number of landmarks
    N                  = 20       # time steps
    world_size         = 100.0    # size of world
    measurement_range  = 50.0     # range at which we can sense landmarks
    motion_noise       = 2.0      # noise in robot motion
    measurement_noise  = 2.0      # noise in the measurements
    distance           = 20.0     # distance by which robot (intends to) move each iteratation 
    
    # Run the full slam routine.
    data = make_data(N, num_landmarks, world_size, measurement_range, motion_noise, measurement_noise, distance)
    result = slam(data, N, num_landmarks, world_size, motion_noise, measurement_noise)
    print_result(N, num_landmarks, result)
    
    # Run the online_slam routine.
    data = make_data(N, num_landmarks, world_size, measurement_range, motion_noise, measurement_noise, distance)
    result = online_slam(data, N, num_landmarks, world_size, motion_noise, measurement_noise)
    print_result(1, num_landmarks, result[0])
    
    # -----------
    # Test Case 1
    testdata1          = [[[[1, 21.796713239511305, 25.32184135169971], [2, 15.067410969755826, -27.599928007267906]], [16.4522379034509, -11.372065246394495]],
                          [[[1, 6.1286996178786755, 35.70844618389858], [2, -0.7470113490937167, -17.709326161950294]], [16.4522379034509, -11.372065246394495]],
                          [[[0, 16.305692184072235, -11.72765549112342], [2, -17.49244296888888, -5.371360408288514]], [16.4522379034509, -11.372065246394495]],
                          [[[0, -0.6443452578030207, -2.542378369361001], [2, -32.17857547483552, 6.778675958806988]], [-16.66697847355152, 11.054945886894709]]]
    
    answer_mu1         = matrix([[81.63549976607898],
                                 [27.175270706192254],