December 14, 2017

• Review of the textbook by Josh Bongard: "This book, however, is not for the faint of heart, as the mathematics required to model and contain this uncertainty may be daunting for some. This book serves well as a graduate textbook, and as a mandatory reference and handbook for those working in the field."

December 12, 2017

• Found A tutorial on Graph-Based SLAM. Maybe I could ask some questions on Fig. 10 or 11.
• Missed the Deep Learning Robotic Challenge in Munich. Could look at SLAM assignment for a question.
• For Probabilistic Robotics, they point to ICRA 2016 tutorial, such as slide 12, slide 22, slide 38 and slide 41.
• For more details Cyrill points to Robot Mapping 2013 (oral exams).

November 19, 2017

• Continue to hone my Python skills with Lesson 12 Quiz 10: program A*.
• My initial solution for Quiz 10 (which returns no output given):
  if g2 < max_cost:
  max_cost = g2
  path[x][y] = delta_name[i]
  
  if found:
  for col in range(len(grid[0])):
  for row in range(len(grid)):
  print path[col][row]
  print "\n"
  It helps when I add path = search(grid,init,goal,cost)
• The line for col in range(len(path[0])): print str(grid[col-1]) + ' ' + str(path[col-1]) gives better output, yet the result is the first chosen node on the boundary, not the best choice:
  for col in range(len(path[0])):
  print str(grid[col-1]) + ' ' + str(path[col-1])
• I would store all travel costs, only update when I find a shorter path and then backtrack from goal to start to create the path. Look what Thrun suggest (as said, the solution is not trivial).

November 6, 2017

• One student used for Question 5.8.4 the model described by ETHZ, minute 7:16. Yet, this is the model of a bicycle with a fixed steering wheel.
• The students provide the movement model, not the probability of a motion.

November 1, 2017

• Since 2016-8 MathWorks has Live Scripts and Live Editor that works like Jupiter notebooks.

October 30, 2017

• Found in the Matlab magazine some routines to visualise the sensor data for automatic driving.

October 24, 2017

• Looked at Freiburg's Exercise 10, which is a paper exercise. Yet, I found the Data Association unnecessary complicated.
• Question 1 from Sheet 10 2016 could be used in the resit.

October 18, 2017

• Found a nice transparant icon of Wall E, with Creative Commons license.
• Did the calculation of question 1b by hand, but I got (0.89 + 0.76 + 0.31 + 0.12)*0.25 = 2.08 * 0.25 = 0.52, while my python-code indicated 0.47. The difference is the normalization. The sum of all probalities is 1.10?! This is again the copy by reference error, with correct initialization there is no need for normalization.

October 17, 2017

• The exams on the website are the partial exam from December 22, 2011 and the resit from February 27, 2012.
• Other exams online are:
  • December 13, 2011
  • December 22, 2010
  • October 15, 2010
  • October 9, 2009
  • October 21, 2008
  • May 26, 2008
  • April 9, 2008
• Should look at Quiz 4 at minute 5:04

October 12, 2017

• The naomanager.py seems to be originating from one of my repositories. It is originating from synchronized dance from Dutch Nao Demo Day. Found the list of participants (i.e. Koen Hindricks), but this list seems not be complete. I miss for instance Sharon and Hessel. Yet, on the video Martin Stolk is operating the synchronized dance.
• Looking in the 2011 Tech Report, I found reference to beaconObs.py created by Hessel.
• Looking if I could use Pattinson (head stuck) for the NaoSLAM task.
• The students have difficulties with connecting, python binaries and the calculating the distance to the landmark.
• Trying to run from Pattinson itself. Used nano as editor. Failed on importing getch. Trying to install it with emerge install py-getch, but nor emerge or pip is known. Should look at preparation for the Future workshop for install clues.
• Download getch-1.0-python2.tar.gz. This contains a c-file and a setup.py. Calling python setup.py build failed because it called i686-pc-linux-gnu-gcc instead of gcc. No gcc in path.
• In C:\projects\Nao\Robocup\Vision\ I have part of the RoboCup code of the DNT from June 2011. The GoalRecog.py contained this interesting snippet:
  def calXangle(xcoord, yawHead):
  #print yawHead
  (width, height) = (160, 120)
  xDiff = width/2 - xcoord
  distanceToFrame = 0.5 * width / 0.42860054745600146
  xAngle = math.atan(xDiff/distanceToFrame)
  return xAngle + yawHead
  
• The GoalRecog.py also contains a function calcGoalDist(filteredblobs, yawHead1, yawHead2):, but this is based on goniometry on the left and right post.
• Looking in the Python code from the Dutch Nao Team. For localisation they used a particleFilter.py. The filter contains a simple motion model and sensor model:
  # Simple motion model: Increment the state by the rotated control vector
  def motionModel( self, sample, control, interval ):
  
  # Add noise to the motion, currently 0.01 meters per 1 meter and 0.2 rad per 1 rad
  control[0] = (control[0] + gauss( 0, 0.01 ) )
  control[1] = (control[1] + gauss( 0, 0.01 ) )
  control[2] = (control[2] + gauss( 0, 0.2 ) )
  
  x = sample[0] + control[0] * cos(sample[2]) + control[1] * cos(sample[2] + 0.5 * pi )
  y = sample[1] + control[0] * sin(sample[2]) + control[1] * sin(sample[2] + 0.5 * pi )
  theta = sample[2] + control[2]
  return x, y, theta
  and sensor model:
  # Sensor model calculates the probability of a state given a measurement
  def sensorModel( self, state, measurement ):
  bearingM, rangeM, signature, idFeature = measurement
  x,y,theta = state
  
  # Only keep a sample if it is in the field
  if -0.7 < x < 6.7 and -0.7 < y < 4.7:
  xR, yR = self.worldMap[idFeature]
  
  rangeR = sqrt( ( x-xR )**2 + ( y-yR )**2 )
  bearingR = self.minimizedAngle( atan2( yR - y, xR - x) - theta )
  
  sigmaRange = 0.3 # range is very uncertain
  sigmaBearing = 0.005 # bearing is very precise
  sigmaSignature = 0.01 # signature does not really matter here
  
  weight = self.prob( rangeR-rangeM , sigmaRange ) * \
  self.prob( bearingR-bearingM , sigmaBearing ) * \
  self.prob( signature, sigmaSignature )
  else:
  weight = 0
  
  return weight
  
• The file locateGoalzor.py containts some hints about the color-range in hsv:
  # Goalfilter values (made in Istanbul 06/07/2011)
  if(color == 'blue'):
  hueMin = 116
  hueMax = 130
  saturationMin = 185
  saturationMax = 255
  valueMin = 75
  valueMax = 210
  if(color == 'yellow'):
  hueMin = 20
  hueMax = 41
  saturationMin = 55
  saturationMax = 255
  valueMin = 60
  valueMax = 210
  
• It also contains a rough estimate of the function pixelsToMeters( pixels): for a [160-by-120] image
  def pixelsToMeters( pixels ):
  # 59 = 0.3
  # 53 = 0.4
  # 44 = 0.5
  # 38 = 0.6
  # 24 = 0.7
  # 17 = 0.8
  # 20 = 0.9
  # 18 = 1.0
  # 12 = 1.1
  # 11 = 1.2
  # 8 = 1.3
  # 5 = 1.9
  # WOLFRAMFIT:
  return 2.55 - 0.56958 * math.log( pixels )
  
• Also found Hessel's code for remote control with 3Dmouse and xbox.

October 11, 2017

• The video on slide 29 of Exploration lecture is displayed at minute 48:00. The video on slide 38 at minute 49;00. The last one (slide 39) at minute 51:50).
• Couldn't find the used video's, but Cyrill has the closest thing: Multi-Robot Exploration Freiburg79, 2008. The video on slide 38 is Information-Driven Exploration, 2005
• The video on slide 32 of 3D Mapping is Octree maps, 2010-2013. The OctoMap video at Handbook of Robotics is the same.

October 10, 2017

• Read Chapter 17 of the Probabilistic Robotics book. It points back to Value Iteration, covered in section 14.3, but I assume that this method is known. It also points back to Augmented Markov Decision Processes, which is a alternative to QMDP. This is covered in section 16.3, together with a particle filter variant of POMDPs called Monte Carlo POMDP (section 16.4). An AMDP maps the uncertainty in motion and sensing to low-dimensial belief, on which it performs exact Value Iteration. This is quite simular with the Monte Carlo Exploration in section 17.2, with the difference that for MC Exploration only the payoff function (Reward) is specified, while ADMP also the transition from one belief to the next is learned.
• Brooke and Tim, report that they don't have a wifi-card once you hit refresh on the web-interface. The fix is on slack: create a wlan.conf. The same problem already occured in April 2010.
• Carlos can be pinged and it starts a connection via Choregraphe (although not visible in the list of active robots, so needed to provide ip-adress). Received the following error message Service "ALChoregraphe" (#1734) is already registered. Rejecting conflicting registration attempt.
• Tried the suggestions of Aldebaran forum: searched for naoqi-bin with ps -u arnoud | grep naoqi, killed the process and restatarted Choregraphe. Could take over control of Carlos. I had to disconnect to give Luca access.

October 9, 2017

• Can start with SeifSLAM, I have still a nice presentation of 24 slides.
• Could be combined with GraphSLAM, another presentation of 25 slides.
• Checking how to cover Chapter 14-16. Freiburg doesn't cover this chapters. Checked the lectures in '09: after FastSLAM they continue with Path Planning and Elevation Maps.
• In Freiburg'17 it has become 3D-Maps, which is actually quite good to show the extension to real applications. Could stop at slide 31, If I can find the 3D Office Building video.
• Path Planning starts with dynamic window approach and value iteration, but from slide 84 it continues with MDPs, where the same scenario as Stanford is worked out.
• Stanford '10 has a combined lecture of 103 slides. Yet, some pruning has to be doned (is Parti-Game covered in the book?).
• The Stanford '10 is much better option than the slides mdps.ppt and pomdps.ppt provided with the book.
• GraphSLAM contains uses as example the GroundHog in the abondended mine. Checked the videos collected under Mine Mapping (the project page is gone, also not recorded with the Wayback machine, yet a copy seems to be available at http://robots.stanford.edu/mines/ together with http://robots.stanford.edu/mines/groundhog/).
  • mine1-anim2.avi Animated 3D mine map (all black)
  • mine1-anim-alt.avi Animated 3D mine map (nice result - good after three video's combined).
  • mine1-video.avi Video of mine mapping robot (initial experiment using indoor robot) - Pioneer2D with 2 SICK laser scans (horizonal and vertical).
  • mine-mapping-live-feed.mpg: footage of the live video feed
  • mine-mapping-first.mpg: footage of the first experiment
  • Manufacture of the Groundhog robot - not interesting for lecture
  • Groundhog video in the Bruceton Research Mine
  • Groundhog video entering the Burgettstown Mine
  • Video from Groundhog exploring the Mathies Mine
  • 6D scan matching (courtesy of Fraunhofer Institute) - short video with matching of 4 3D laserscans
  • Crude 3-D model of the Bruceton Research Mine
• First watched three videos combined. 2 minutes and a lot of mud.
• Also found SegBot page including outdoor exploration on Thrun's research page.
• FastSLAM seems not be applied to RoboCup Soccer Fields, the first hits are min (). The only other one is Natural landmark localisation for RoboCup, which is only localization but refers for mapping to FastSLAM.
• Both lectures were a bit short (half an hour instead of 45min). The SeifSLAM lecture could use some more detail about the four steps of the algoritme and with which equation the information matrix is sparsified.

October 7, 2017

• Made the six landmarks for the last assignment.
• In the 2013 Rules the landmarks were placed on top of the wall. The distance between the outer edges is 310cm, the walls are 20cm thick, the landmarks were in the middle, so the centers are 290cm form each other.
• In the 2013 Rules the landmarks were placed 15cm behind the lines.
• The current field has the T-junction on precisely 199.5cm (outher edge on 20cm). The L-junction is at 297cm (outher edge on 300cm).
• Put black markers on the ground, at (0,215),(0,-215),(315,215),(315,-215),(-315,215),(-315,-215).
• Checked the field setup on page 24 of Landmark-based Localization.
• Put black markers on the ground at (0,0), (100,0), (100,-100), (-150,0), (-150,-100).
• Started on nb-ros choregraphe v2.14.13 (with bin/choregraphe-launcher).
• Started to work with Carlos, but he couldn't connect to the network.
• Strange, because I can connect with my nb-udk. Could connect to the webinterface, both robolab (146.50.60.47) and wired (169.254.35.27). Still, nb-ros couldn't connect to NAOqi.
• Couldn't ping to 146.50.60.47 from nb-udk, but that was because it tried to connect to reach it via the wired route. Logged in from nb-udk through the wired connection and did a nao restart.
• Made on nb-udk in Choregraphe an project which takes a picture at each waypoint. First waypoint is (0.5,-0,5,45), (0.707,0,0), 3x(0,0,-45), (1.0,0,0), 2x(0,0,-45), (1.0,0,0). Should also take a picture before start and move 0.5m after the second and third mark, but lets try this first.
• Made also a script for the other 2 points. That direction the robot makes a larger odometry error; I had to manually correct him.
• I have several good images of each landmark:
     
              top-yellow                                           top-green                                    top-blue
     
              bottom-yellow                                bottom-green                                bottom-blue                
  

October 5, 2017

• Looking for the video material from the Handbook of Robotics. Wolfram wrote the chapter World Modelling with a few 3D mapping video's.
• Sebastian worked on the chapter on SLAM. One of the video's is the EKF SLAM on the MIT tennis court.
• Dieter Fox didn't write a chapter, but the Probabilistic Robotics book is often cited (together with his other work). I am cited twice in the handbook.
• I got the hint that there is a bug in the EKF.m, because when there is a mark in the logfile, a move to position 0,0 is added to the list.
• Tried to solve this bug with the code:
  if skipped == 1
  xt(1,t-1) = (xt(1,t) - xt(1,t-2))./2; %halfway
  xt(2,t-1) = (xt(2,t) - xt(2,t-2))./2;
  xt(3,t-1) = (xt(3,t) - xt(3,t-2))./2;
  skipped = 0;
  end
  Unfortunatelly, both EKFSLAM and EKFloc2 do not run on my laptop with Matlab R2012b.
• Made a small +/- error in the above code. Corrected after inspecting it with the print statements, which also revealed that this code wasn't needed because the skip works fine:
  if skipped == 1
  fprintf(1, 'mark at time %d, was given position (%.2f,%.2f,%.2f)\n', t, xt(1,t-1),xt(2,t-1),xt(3,t-1));
  xt(1,t-1) = (xt(1,t) + xt(1,t-2))./2; %halfway
  xt(2,t-1) = (xt(2,t) + xt(2,t-2))./2;
  xt(3,t-1) = (xt(3,t) + xt(3,t-2))./2;
  fprintf(1, 'mark at time %d, approximate position (%.2f,%.2f,%.2f)\n', t, xt(1,t-1),xt(2,t-1),xt(3,t-1));
  fprintf(1, 'mark at time %d, previous position (%.2f,%.2f,%.2f)\n', t, xt(1,t-2),xt(2,t-2),xt(3,t-2));
  fprintf(1, 'mark at time %d, current position (%.2f,%.2f,%.2f)\n', t, xt(1,t),xt(2,t),xt(3,t));
  skipped = 0;
  end
  

October 4, 2017

• Read Chapter 13 of the Probabilistic Robotics book. Key phrase on page 441: "Knowledge of the exact location of one feature will therefore tell us nothing about the locations of other features, if the robot path is known".
• The video on slide 30 of FastSlam (feature based) seems to be montemerlo-fastslam-aaai, which is available as additional material of Springer Handbook of Robotics.
• That video is not available from Thrun's video page, nor from Cyrill Stachness video page. Yet, there are some nice GraphSlam video's at Cyrill's page. In the original powerpoint, the video is called aaai06.avi, from stachniss amsvl05-fastslam talk.
• The video on FastSLAM at the Victoria Park dataset seems to be based on this ICRA'03 paper. The video is visible in the recording at 38:50. In the original powerpoint, the video is called vic-nege.avi, from stachniss amsvl05-fastslam talk.
• In the original powerpoint, the first grid-based video is called bruceton-one-loop.avi, from mine-mapping.
• In the original powerpoint, the second grid-based video is called bruceton-rb-mapping-rle.avi, from mine-mapping.
• In the original powerpoint, the third grid-based video is called haehnel-ScanMatchingFastSLAM-Seattle-anim-2.avi, from darpa-mars03.
• Actually, it are three video's on top of each other. Downloaded all three to my Stanford09 folder: nicely works.
• The first grid-based video in Wolfram's grid-based SLAM lecture seems to be (minute 2:51) Raw data from Intel Research Lab. Freiburg has many more video's, but I haven't time to find them all.
• The students asked why in the prediction step of the provided code the motion model is different from the motion model in the book. I answered because it is not the velocity model from section 5.3. Yet, in my Guttman paper I directly refer to the velocity model from section 5.3. Yet, it is also not the odometry model of section 5.4. The paper from Kristensen doesn't contain a motion model. Checked Gutmann continued paper. The original paper contains nice measurements for the sensor model, but no information about the motion model.
• According to the comments in the code, it is made in 2008 based on Thrun's and Wolfram's slides. Wolfram's slides are about EKF-localization, not SLAM. They specify the motion function g and the Jacobian as specified on page 205 (whch referrs to the velocity model of section 5.3.
• Also in Hessel's thesis the motion model is not mentioned.
• On my new Windows10 installation there is Matlab R2014b installed.
• Hessel's code still works, with call EKFSLAM(1,R,Q,1.5,10). The R is defined as R = eye(3) * 0.005, the Q as Q = eye(3) * 0.1; Q(1,1) = 0.17; Q(2,2) = 0.38.
• With R = 0 the green landmark is drawn far from the rest (but the path still looks OK). Yet, all information is collected in the muHistory, but it is a bit unclear how the landmarks are effected. Yet, if I return muHistory, it is a 21x1x758 double. The landmark positions are drawn from landmarkX = muHistory([4 7 10 13 16 19],1,t); landmarkY = muHistory([5 8 11 14 17 20],1,t);.
• Yet, it seems that the code I provided only contains the approximation when da=0 (and with x,y defined differently than table 5.3). The solution of Moos nicely makes the destinction for the two cases:
  if da == 0
  mmat = [v*sin(x_(3)); ...
  v*cos(x_(3)); ...
  0];
  gmat = [0 0 v*cos(x_(3)); ...
  0 0 -v*sin(x_(3)); ...
  0 0 0];
  else
  mmat = [-(v/da)*sin(x_(3)) + (v/da)*sin(x_(3) + da); ...
  (v/da)*cos(x_(3)) - (v/da)*cos(x_(3) + da); ...
  da];
  gmat = [0 0 -(v/da)*cos(x_(3))+(v/da)*cos(x_(3) + da); ...
  0 0 -(v/da)*sin(x_(3))+(v/da)*sin(x_(3) + da); ...
  0 0 0];
  end
  
• Looked again at . The values in dlog.dat are x,y,theta which are the absolute positions. The first mark is given at (384, -349, 85)mm/deg, which should be the position (50, -50)cm (t=36045). The second mark is (122,104,-77)mm/deg, which should be (50,0)cm (t=38870). The third mark is at (-516,-80,-163)mm/deg, which should (0,0)cm (t=40804).
• The pose.log.SRL gives at t=36045 (458,-439,76)mm/deg, t=38870 (374,89,-97), t=40804 (-12,-153,-161)
• The fourth mark is given at t=42643. The dlog.dat gives (-1320, -38, 179), the reference gives (-1063, 3, 166). So, this is point (-100,0)cm.
• The fifth mark is given at t=46310. The dlog.dat gives (-1267, -307, 60), the reference gives (-998, 585, 0). So this is point (-100,50).
• The x and y are converted to dx and dy, which are converted to r^'. Yet, this is not the r^* from Table 5.1, which calculates the rotation around the ICC as indicated with x^* and y^*. Instead, it calculates the movement as x + r cos(da) and y - sin(da). To get omega and v one needs to calculate all steps of Table 5.1. Instead, the predicted robot position implements lines 8-9 from Table 5.6, or equation 5.40 at page 139 (without d_{rot2}).
• Corrected the equations with the start_angle (instead of the diff_angle). Results seem to improve. When I use a small R (0.001), the green landmark is far off the field. With bigger R (0.02) there is wiggling visible in the path. With R (0.01) the result seems OK.
   
• The result of the localisation at the first mark is (4.06,-4.48,1.48), 50cm from pose.log.SRL gives at t=36045 (458,-439,76)mm/deg. With the new code (4.00, -4.13, 1.48).
   
• At the second point the new code gives (1.40, 0.7, 4.92), instead of (374,89,-97)
  
• For the third point (-4.2, -1.8, 3.42), instead of (-12,-153,-161).
   
• The last result looks good, except for the Z below the full circle near 0,0. Looks like a 180deg error for a few steps.
• Increasing the R from 0.01 to 0.02 makes it only worse: (-4.45, -1.9, 3.42). Reducing R to 0.001 makes it resamble the recorded path (which is not the ground truth (-4.93, -1.04, 3.42). Calculating Q with 0.17 and 0.38 (instead of 0.2 and 0.3 improves the result a bit (-4.2, -1.8, 3.42). Reducing Q(1,1) to 0.1 doesn't change the result much.
• Increasing the R to 0.1 gives an (ugly) result (-0.31, -0.56, 3.42).

October 3, 2017

• Read Chapter 13 of the book. Key phrase on page 441: "Knowledge of the exact location of one feature will therefore tell us nothing about the locations of other features, if the robot path is known".

October 1, 2017

• Jonas Köhler found a second order derivation with R = [1/3 1/2; 1/2 1]. This same chapter indicates that R = [1/4 1/2; 1/2 1] is for the case that the noise between the sample timesteps is constant. Unfortunatelly, the mathematical derivation of both R-matrices is not given.

September 28, 2017

• A good question of Muriel on the validation gate. This includes a sandwich `of the innovation vector and the measurement covariance, as defined by Kristensen. Why the measurement covariance?
• Cannot find it in the book, although I remember reading it.
• Validation gates are mentioned in SLAM lecture, without explanation.
• Found the same equation in SLAM for dummies, page 27. The rational is to check if the landmark is within the area of uncertainty.
• In my Guttman paper i point to the Mahalanobis distance used by Hugh F. Durrant-Whyte.
• Found the answer in "Estimation with Applications to Tracking and Navigation: Theory, Algorithms, and Software", page 166. The magic number 2 indicated the 2-sigma of the confidence region.
• Another good question is on Table 10.1: the Sigma_t is size (3N+3)x(3N+3), while the slides indicate 2N. Yet, this is for the Sigma after the update.

September 26, 2017

• Showed this video from CMU on how the camera view on a soccer field looks like.

September 22, 2017

• for j in range(0,100):
  change = 0.0
  for i in range(1,len(path)-1):
  newpoint = newpath[i]
  datapoint = path[i]
  previous = newpath[i-1]
  next = newpath[i+1]
  oldpoint = newpath[i]
  newpoint[0] = newpoint[0] + weight_data * (datapoint[0] - newpoint[0]) + weight_smooth * (next[0] + previous[0]- 2 * newpoint[0])
  newpoint[1] = newpoint[1] + weight_data * (datapoint[1] - newpoint[1]) + weight_smooth * (next[1] + previous[1]- 2 * newpoint[1])
  change += math.sqrt(math.pow(newpoint[0] - oldpoint[0],2) + math.pow(newpoint[1] - oldpoint[1],2))
  The result seems to be OK:
  [0.000, 0.000] -> [0.000, 0.000]
  [0.000, 1.000] -> [0.021, 0.979]
  [0.000, 2.000] -> [0.149, 1.851]
  [1.000, 2.000] -> [1.021, 1.979]
  [2.000, 2.000] -> [2.000, 2.000]
  [3.000, 2.000] -> [2.979, 2.021]
  [4.000, 2.000] -> [3.851, 2.149]
  [4.000, 3.000] -> [3.979, 3.021]
  [4.000, 4.000] -> [4.000, 4.000] With while (change > tolerance): the loop also works, converts in 7 steps. Still, Thrun doesn't says it isn't the expected result. That is true, his output gives [0.029, 0.971] for the first point! Difference is that I used Euclidian distance, while Thrun uses Manhattan distance (change += abs(aux - newpath[i][j])
• Yet, a change to Manhattan distance change += abs(newpoint[0] - oldpoint[0]) + abs(newpoint[1] - oldpoint[1]) didn't improve the result. Actually the first point is not adjusted at all:
  [0.000, 0.000] -> [0.000, 0.000]
  [0.000, 1.000] -> [0.000, 1.000]
  [0.000, 2.000] -> [0.100, 1.900]
  [1.000, 2.000] -> [1.010, 1.990]
  [2.000, 2.000] -> [2.001, 1.999]
  [3.000, 2.000] -> [3.000, 2.000]
  [4.000, 2.000] -> [3.900, 2.100]
  [4.000, 3.000] -> [3.990, 3.010]
  [4.000, 4.000] -> [4.000, 4.000]
• The solution of Thrun, with a double loop is more elegant:
  for i in range(len(path)-1):
  for j in range(len(path[0]:
  aux = newpath[i][j]
  newpath[i][j[ += weight_data * (path[i][j] - newpath[i][j) \
  + weight_smooth ( newpath[i-1][j] + newpath[i+1][j] - (2.0 * newpath[i][j])
  
• Quiz 9 is really simpel, if you don't forget to update of myrobot from the move command (and use a minus for the steer):
  for i in range(N):
  steer = param * myrobot.y
  myrobot = myrobot.move(-steer,speed)
  

September 21, 2017

• Typesetting the answer on Exercise 5.8.4 with algorithmicx. This package depends on algorithms.
• Found an article with the kinematic model of the bicycle, with pointed for the model to Lateral control of a backward driven front-steering vehicle as source of the model.
• The Exercise looks at the exam question with the curvature.
• Calculated r^* like Table 5 - line 5, when I use x^* = x - 1/gamma sin theta and y^* = y + 1/gamma cos theta (as the kinematic model of the bicycle and the result is sqrt ( 1/gamma^2 cos^2 theta + 1/gamma^2 sin^2 ) = 1/gamma, as it should.

September 20, 2017

• Udacity announces Flying Car Nanodegree by Nicholas Roy. It is based on flight simulator, but on github I only saw the Unity simulator for the RoboND. The class will open early 2018!
• Willow Garage closed in 2014. The PR2 is now Research service of Clearpath Robotics, but the wiki is not longer functional, only the list of repair manuals.
• Searched for cornerflags. I only found three: pink-blue, pink-yellow, blue-pink. So I should make yellow-pink, green-pink, pink-green.
• Looked at 2013 Rules, but they point to the document 'Colours used in the football field'.
• In 2004 the green-pink, pink-green cornerflags are gone.

September 19, 2017

• Looked at Motion and Sensor Assignment from Fall 2008. This is assignment is in 2010 replaced by an Extended Kalman Filter Assignment.
• An alternative is question 5.8.4 and 5.8.5.
• An other alternative is Freiburg's Locomotion exercise, but the solution is given: here.
• For the sensor model Freiburg has sensor model exercise, including solution.
• To draw a vectorfield in Matlab, you could use quiver.

September 18, 2017

• Updated the links to the Motion Model lectures of today. As PowerPoint, I have the motion-models.ppt from 2007, the powerpoint wheel-locomotion should be given up-front.
• The original motion-models.ptt has 33 slides, the current motion-models.pdf has 43 slides.
• The original motion-models.ptt has 17 slides, the current 03-locmotion.pdf has 23 slides.
• Instead, I use Siegwart slides (36 slides).
• Checked the video's on mobile robots site, but none of them is from Willow Garage.
• Also couldn't find it on Scaramuzza's site.
• Found Borenstein.mpg (Siegwart's slide 27).
• Using Siegwart's lecture didn't work that well; too much detail - needed full hour. Next year, back to 03-locomotion.pdf.

September 17, 2017

• The hint was to keep the thresholds of Canny edge in the tents, but at the end I submitted a threshold above 100 to Quiz 12:
  # Define our parameters for Canny and run it
  low_threshold = 80
  high_threshold = 120
  edges = cv2.Canny(blur_gray, low_threshold, high_threshold)
  Result was quite good:
  
• Had to add plt.show() at the end of the file to see the result of Canny.py and Region.py from the commandline and Idle.
• Found the following parameters for the Hough-Quiz:
  # This time we are defining a four sided polygon to mask
  imshape = image.shape
  vertices = np.array([[(80,imshape[0]),(430, 300), (520, 300), (900,imshape[0])]], dtype=np.int32)
  cv2.fillPoly(mask, vertices, ignore_mask_color)
  masked_edges = cv2.bitwise_and(edges, mask)
  
  # Define the Hough transform parameters
  # Make a blank the same size as our image to draw on
  rho = 1 # distance resolution in pixels of the Hough grid
  theta = np.pi/180 # angular resolution in radians of the Hough grid
  threshold = 3 # minimum number of votes (intersections in Hough grid cell)
  min_line_length = 15 #minimum number of pixels making up a line
  max_line_gap = 3 # maximum gap in pixels between connectable line segments
  line_image = np.copy(image)*0 # creating a blank to draw lines on
  Result was quite nice:
  
• The final project of this lesson can be found on github.
• Following the instruction of Term 1 Starter Kit and tried to install Anaconda.
• Should have checked conda update conda, but now installed a ~/miniconda3 and modyfied my PYTHONPATH.
• Continued with the instruction and created a new conda environment conda env create -f environment.yml.
• This installs many packages in a seperate environment. Have to start to be carefull, because 209 Gb is used, 11 Gb remaining.
• Seems to be created succesfully, I so no warning and could do source activate carnd-term1.
• Performed conda clean -tp and removed 450 Mb on tarballs.
• Run the iPython notebook and all seem to be up-and-running.
• No way to perform or submit the first project, although this should continue in Project 1 notebook.
• So the projects of the NanoDegree for term 1 are:
  1. The Finding Lane Lines project.
  2. The Behavioral Cloning project.
  3. The Advanced Lane Line and Vehicle Detection project.
• So the projects of the NanoDegree for term 2 are:
  1. The Sensor Fusion project.
  2. The Localization project.
  3. The Controllers project.
• So the projects of the NanoDegree for term 3 are:
  1. The Path-Planning project.
  2. The Real self-driving project.
• The Capstone project is based on a ros-based simulator and a ros bag with measurements.
• Running pip install -r requirements.txt.
• Fails on urllib3.

September 16, 2017

• Thrun's motivation with the car acident can be found at YouTube.
• The NanoDegree has .
• The NanoDegree has 7 projects of a month, and one project of a week (Finding Lane Lines), which is available in Preview.
• Did Lesson 2 - Quiz 7, and changed the mask to:
  # Define the vertices of a triangular mask.
  # Keep in mind the origin (x=0, y=0) is in the upper left
  # MODIFY THESE VALUES TO ISOLATE THE REGION
  # WHERE THE LANE LINES ARE IN THE IMAGE
  left_bottom = [120, 535]
  right_bottom = [825, 535]
  apex = [460, 315] Result was quite nice:
  

September 14, 2017

• In November 28, 2011, I was struckling to find a nice introduction to localization without redoing EKF and PF.
• Lesson plan for today:
  • Lesson 8.1 Field Trip. Visit to Smitsonian. Not that informative, but 2 minutes so a good warm up.
  • Lesson 8.5 Particle Filters. Compared agains HF and KF.
  • Lesson 8.14 Importance Weight. Recap of yesterday's class in 5 minutes. Stop at 2:56. The Quiz is to make a vector of 1000 importance weights.
  • Lesson 8.25 Filters.Conclusion, stop at 2:12. Quiz about his job-interview.
• With this summary, not much is said about the movement model. The final code creates, moves and measures. Than the particle kicks in, with calculating the imporance weights and the resampling. The move function not only moves the robot, but also includes randomness to the move (and sense function). This is done in Quiz 10 (without any rational, just as programming exercise).
• Julian had on November 9 also hints for Assignment 3.8.1:
  

  Uit de feedback in de klas begreep ik dat iedereen geprobeerd heeft om de acceleratie in de matrix B en action u_t te verwerken. Dus ze dachten dan dat de acceleratie van de N(0, 1) gesampled moesten worden, en voerden dan een soort simulatie uit. Na de klassikale bespreking snappen ze nu wel dat dat juist niet de bedoeling is aangezien je je belief distributie over de state wel hebben, maar eigenlijk gaf iedereen aan dat ze dat nooit zelf bedacht zouden hebben. Wat namelijk helemaal lastig voor ze is, is om de afleiding te maken dat:

  e_t = [1/2 1]' * a_t, waar a_t ~ N(0, 1) => vindt nu R zodat e_t ~ N(0, R)

  dan

  R = [1/2 1]' * [1/2 1] = [1/4 1/2; 1/2 1], aangezien je moet weten dat

  (1) Y = A X + B, X ~ N(m, S) => Y ~ N(A * m + B, A'* S * A')

  Dit soort regels kennen ze niet (meer?), ik weet niet of het wel in hun curriculum zit (kennelijk bij Thrun's studenten wel).

  Ik denk dat het dus belangrijk is dat we dat volgend jaar als hint bij de opgave zetten.

• The physics behind the application is explained on wikipedia, including Newton's laws of motion and the covariance from (1/2 dt^2, dt).
• I found Julian's solutions of 3.8.1 and 4.6.1 (partly in my mail-archive, partly on H).
• Added code for 3.8.2 to Julian's solution. With Q=[10 0; 0 0]; there exist no inv(S). With Q=[10 0; 0 1]; you get K = [0.8 0.0; 0.2439 0]. Got another error, but that was a foregotten '*'. Solved it by calculating the intermediate results (the innovation vector y = z - C * mu was nicely [5 0], after multiplying with K [4.02 1.21]. It is OK that the observation leads to a speed, but I would have imagined a speed of 4/5 = 0.8 .
• Emiel indicated that it is nicer to use C=[0 1], Q=10 and z=5, because otherwise you claim that you measure a zero speed. Adjusted the code, works also fine:
  
• In Lesson 4.26 Kalman Matrices, Thrun uses a CholeskyInverse for the inverse, which takes first the Cholesky upper Triangle, before the inverse. Matlab has several inverses, but many are part of DSP System Toolbox.
• Yet, the Sigma after the observation was bigger, but that was because I used I = ones(2) instead of I = eye(2).
• Looked at the solution for Assignment 4.8.1. Julian had some problems with the singular R. Proposal from the Msc-students is to use R = [1/3 1/2;1/2 1]. With this R the result is much more concentrated around 0.0:
   
• During class, I got questions about the volume of region x_{k,t} on page 89. In Wolfram's class this was not explicitly mentioned. Should look at Dieter of Thrun's original powerpoints. The original powerpoint is even shorter. No help from Dieter, he falls back to Bayes rule. His occupancy mapping presentation contains nice examples of effects of resolution on 3D maps.
• The Gaussian membership function g = gaussmf([0 0]' - [5 0]',Q) returns a vector 0.88 1.0. Should use the x, \dot{x} from Im. The maximum of Im is in the middle (Im(6,11)). This could be found in P2 with P(6+11*(bins(2)-1)).

September 13, 2017

• Slide 12 of Particle Filters lectures explains importance sampling. The first two distributions I understand, although it is good to show equation 4.25 on the board. The third has to be derived. This is done in section 4.3.3, but this is not the same as on slide 12. To derive the third equation one need to show this on the board:
  
• If they want to get deeper into Importance Sampling, I could discuss this CMU lecture.
• Importance Sampling is covered in chapter 11.1.4 of Bischop, on page 532-534.
• Slide 20 is messed up. I have the good one in particle-filters.ppt, but that presentation is from 2007 (and is missing some slides, such as slide 12). Used instead the presentation from 2016.
• Looked at previous Matlab solution (SanDisk->ProbRob->Matlab). The Kalman Filter for the Aibo field was initiated with xt(:,1) = [0 0 0]';. Yet, the mean and covariance are not collected in an array, but calculated from the previous value. Only at the end the mean and covariance are collected: x(:,t) = x_; P(:,:,t) = P_;.
• Finaly I have some running Matlab code:
  % Assigment3.8.1c Implement the prediction step of a Kalman filter
  X_initial = [ 0 0 ]; % no movement
  A = [ 1 1; 0 1];
  B = [ 0 0; 0 1];
  R = [ 0 0; 0 0];
  
  mu(:,1) = X_initial;
  Sigma(:,:,1) = [ 1 0 ; 0 1];
  
  for i = 2:6
  mu(:,i) = A * mu(:,i-1) + B * [0 1]' * randn()
  Sigma(:,:,i) = A * Sigma(:,:,i-1) * A' + R
  end
• On ScanDisk->ProbRob->MotionModel a Matlab implementation of Table 5.1 (except the probabilities).
• Seems to be no other Matlab files on the Gaming pc. Correction, found the gravity test of UsarNao and the demo of the EM shift of Zoran.
• In addition on the Gaming PC I also found the Gaussian Mixture Model from sroebert.
• Also found Matlab code from mliem and ndijkshoorn.
• In 2009/assistance I found a map LandmarkDetection.
• Also found MatlabAnalysisModule of mpfingsthorn, which analysis walls.
• Although C:\Programs\MATLAB701 seems not mounted, it actually is. Copied work. No license to run it anymore :-(.
• In aibo-slcmp there is another implementation of KF (with 758 points instead of 300, because the first mark is at 500).
• In aibo-slcmp there is also a EKF.m implementation.
• In 2011 I have several solutions of the students for the EKF assignment, including drawCov2.m. The difference from drawCov.m is that you can select the color of the plot.
• In the WarmingUp_vdveen is no Kalman filter, but there is a particle_filter.m
• Further no Matlab-code on my ScanDisk.
• The drawCov2.m is called by Hessel with:
  for landmark=1:N
  if (mod(landmark,cov_mod) == 0)
  colorshift = (cov_mod/N) * landmark/cov_mod;
  drawCov2(x(1:2,landmark),P(1:2,1:2,landmark),[sin(pi*colorshift),colorshift,1-colorshift]);
  end
  % if z(1,landmark,i)~=0
  % plot([xt(1,landmark),mlocs(1,landmark)],[xt(2,landmark),mlocs(2,landmark)],[col(i) ':'],'Linewidth',.5);
  % end
  end
  
• So, for my assigment3.8.1c, this translates in:
  hold off
  for t=1:6
  drawCov(mu(1:2,t),Sigma(1:2,1:2,t));
  hold on
  end
  hold off
  
• The result is a bit weird, because my Kalman prediction moves (due to adding the random acceleration):
  

September 12, 2017

• Nice quote of Rodney Brooks:

  "Robotics is where Artificial Intelligence collides with the un-sanitized natural world. Up until now the natural world has been winning, and will probably continue to do so most of the time for quite some time."

• All answers in Fall2008 at u033089 are for Assignment 2.8.4.
• Tried to get a nice list of vectors with cell, but that didn't work:
  % Assigment3.8.1c Implement the prediction step of a Kalman filter X_initial = [ 0 0 0 ]'; % no movement
  A = [ 1 1 0 ; 0 1 1 ; 0 0 0 ];
  R = [ 0 0 0 ; 0 1 1 ; 0 0 1 ];
  
  mu = cell(1:6);
  mu(1) = {X_initial};
  Sigma = cell(1:6);
  Sigma(1) = {[ 1 0 0 ; 0 1 0 ; 0 0 1 ]};
  
  for i = 2:6
  mu(i) = A * cell2mat(mu(i-1))
  end
• Lesson plan for today:
  • Lesson 4.25 Kalman Filter Design.
  • Discussed Exercise 3.8.1 & 3.8.2. Students game with good suggestion to model the model noise R_t with \delta \delta^T, where \delta = (1/2 \Delta t^2, \Delta t), which is the second Tayler extension. Needed only one hour for two exercises. 3.8.2 is a bit non-logical, because after 5 timesteps you observe z=5, which works much better if you had an initial speed of nearly 1.
  • With an initial speed of non-zero, you also see the Kalman updating the mu's.

September 11, 2017

• I could continue with kalman.ptt, from slide 22.
• Stanford CS226B-04 has working slides with the lighthouse. Could use it as introduction :-) to slide 22.
• ExtendedKalmanFilters uses this example at least (slide 9).
• I can find the ExtendedKalmanFilters.pdf on my Kingston, but not the ppt.
• Made a new ExtendedKalman.ppt on my ScanDisk. Both EKF as UKF is covered (including Aibo example). Couldn't find Information Filter slides (as in ExtendedKalmanFilters.pdf). Is also not included in the slides at www.probabilistic-robotics.org, although the date is from 2016.
• Information Filters are also not available in Freiburg 2017 EKF lecture, but this lecture contains nice reminder on the Jacobian Matrix.
• This is the result.
• Will also try to cover Choset's lecture today.
• Homework for tomorrow is 3.8.1 and 3.8.2. Graded will be 4.8.1 and 4.8.4 (or even better 4.8.2 and 4.8.5)?
• Changed the assignments to unlimited attempts. This option is not available in the Grading Centre, only in the Grading Options when you edit the assignment in the Content area.

September 7, 2017

• Plan for today the practical session of today:
  • Lesson 4.1 Introduction.
  • Lesson 4.2: Especially point towards the elipses of the cars.
  • As said on August 26: unbelievable low level. Quiz 3-5 can be skipped.
  • Directly jump to Lesson 4.10! Good intro, easy question.
  • Lesson 4.11 is also not insane difficult, but short enough to have a smooth transition to 4.12, which repeats yesterday's lecture.
  • Lesson 4.13: Answer 12.4 and 1.6.
  • Lesson 4.14: Too simple, but luckely short.
  • Lesson 4.15: "Hard question". also short!
  • Lesson 4.16: translate the Python program to Matlab!
    function [new_mean, new_variance] = update(mean1,variance1,mean2,variance2)
    new_mean= (mean1 * variance2 + mean2 * variance1) / (variance1 + variance1);
    new_variance = 1 / (1 / variance1 + 1 / variance2);
    Can be called with [mu,sigma] = update(10,8,13,2)
  • Lesson 4.17: Could be used as round up, but I think better to make after 4.16 the transition to 2.8.2.
  • The answer on 2.8.2a is simple:
    transition = [[.8, 0.2, 0.0];[.4,.4,.2];[.2,.6,.2]]; % 1 is sunny
    % 2 is cloudy
    % 3 is rainy
    sequence_2a = transition(1,2) * transition(2,2) * transition(2,3)
    
  • The answer on 2.8.2b is given:
    % MATLAB script
    % y = sample(x) returns y sampled from distribution x
    X_today = [ 0 1 0 ]'; % Cloudy
    T = [ 8 2 0 ; 4 4 2 ; 2 6 2 ]' / 10;
    X_tomorrow = sample( T * X_today );
    if ( X_tomorrow(1) == 1 )
    disp( 'sunny' );
    elseif ( X_tomorrow(2) == 1 )
    disp( 'cloudy' );
    elseif ( X_tomorrow(3) == 1 )
    disp( 'rainy' );
    end
    
  • Yet, sample is no longer a function, did they intend stats::sample?
• I have the diary of Assignment 2.8.4, but still have to make it for Assignment 2.8.2 and 2.8.3.
• The diary of Assignment 2.8.4 has some initial trials, but still the distribution as a function of both x and z, not with initiated prior or observation:
  h = linspace(400,1600,51);
  v = linspace(400,1600,51);
  [X,Y] =meshgrid(h,v);
  [X,Z] =meshgrid(h,v);
  pz = exp((-(1000-10*X+9*Z).^2)/18000)/(6*sqrt(5*3.1457));
  surf(X,Z,pz)
  figure(2)
  pzz = exp(-(-1000+X).^2/1800+(-1000+Z).^2/2000-(-X+Z).^2/200)/(6*sqrt(5*3.1457));
  surf(X,Z,pzz)
  
• On u033089.uwp.science.uva.nl I have a Wolfram Notebook File with I seem to have used for my Answer2.8.4. So no ProbRob files in my matlab folder on pc jose nor on my home.uva.nl drive. There was another Assignment2.8.4Answer on my home.uva.nl:Linux drive, together with an ErrorVarianceAssignment in Fall2008 (based on UsarSim).
• In Ethem's Introduction to Machine Learning, Kalman Filters are covered in the chapter about Hidden Markov Models, as the case when not only the observations but also the state is continues, but this is not covered in his slides.

September 6, 2017

• There is a short break in the state estimation recording after 43min.

September 5, 2017

• Plan for today the practical session of today:
  • Lesson 1.1 Introduction.
  • Lesson 1.3 Total Probability
  • Lesson 1.7 Probability after sense (0.04, 0.12, 0.12, 0.04, 0.04).
  • Skip Lesson 1.8 Compute Sum (0.36).
  • Show Lesson 1.9 Normalize (0.11, 0.33, 0.33, 0.11, 0.11)
  • Jump to Lesson 1.13 Normalize Sense function:
    p=[0.2, 0.2, 0.2, 0.2, 0.2]
    world=['green', 'red', 'red', 'green', 'green']
    Z = 'red'
    pHit = 0.6
    pMiss = 0.2
    def sense(p, Z):
    qu=[]
    for i in range(len(p)):
    hit = (Z == world[i])
    qu.append(p[i] * (hit * pHit + (1-hit) * pMiss))
    t = sum(qu)
    q=[]
    for i in range(len(qu)):
    q.append(qu[i] / t)
    return q
    print sense(p,Z)
    
  • Test this form multiple measurements (Lesson 1.15).
  • Show this in Matlab:
    function q = sense(p, Z)
    world = {'green','red','red','green','green'};
    pHit = 0.6;
    pMiss = 0.2;
    hit = strcmp(world,Z);
    q = p .* (hit * pHit + (1-hit)*pMiss);
    s = sum(q);
    q = q / s;
    return
  • Then jump to Exercise 2.8.1
  • Then jump to Exercise 2.8.2
• Continue state estimation recording after 35min.
• The next exercises are 3.8.1 and 3.8.2.
• I am missing a link to the Kalman lecture, although that probably overlaps with EKF. Repaired it. In my slides I am missing slide 7& 8, about the components of the Kalman Filter and derivation. I prever the approach of Wolfram.
• In Freiburg the second exercise is mostly about Linear Algebra, although the third question is to plot laserscan-data. The previous year the exercise was given both in Python and Matlab, without the solution.
• Should look at Octave setup and come up with a Matlab script with the same functionality as Python.
• Matlab solution for first part (no return in Matlab functions):
  function [ Y ] = f( X )
  Y = cos(X) .* exp (X)
  end
  
• Also the second part is easy:
  x = -2*pi:2*pi;
  y = f(x);
  plot(x,y)
  print('Exercise2','-dpng')
  I only have to add some additional information. In Python they added:
  plt.grid()
  plt.ylabel('cos(x) * exp(x)')
  plt.xlabel('x')
  which is simply:
  xlabel ('x');
  ylabel ('cos(x) * exp(x)');
  grid ('on');
  
• Lesson 12 is very easy (at least until Quiz 7). Also not very relevant, I assume that our students can program A*.
• Need to do quite some programming for First Search Program (Quiz 8). The first stub produces the right answer (by cheating), but the answer is not accepted (with or without point behind the path):
  def expand(node):
  cost = 11
  triplet_list = []
  triplet = []
  triplet.append(cost)
  triplet.append(node[0]) # should be delta
  triplet.append(node[1]) # should be delta
  
  triplet_list.append(triplet)
  return triplet_list
  
  def search(grid,init,goal,cost):
  path = expand(goal)
  return path
  path = search(grid,init,goal,cost)
  print str(path[0]) + '.'
  The format seems alright, but I assume that it complains about other test-grids.
• Continued with Quiz 9, which is a few lines of code:
  expand_id = -1
  expand = [[-1 for row in range(len(grid[0]))] for col in range(len(grid))]
  while not found and not resign:
  expand_id += 1
  expand[x][y] = expand_id
  Thrun initiates his count with 0, because he does += after the assignment.

September 4, 2017

• Looking for the original of RobotParadigms, but couldn't find the PowerPoint. Yet, it seems that it originates from Freiburg; 02-paradigms.pdf, yet the layout is from Stanford. The Paradigms is part 2 of CS226B-01-Introduction (Scandisk3 -> Stanford). Only slide missing is 1-5 (AI View on Mobile Robotics). Could add Freiburg lecture 2 as recording.
• For the StateEstimation I found several version (online is the one from 2010), but the CS226B-02-Probabilities from Stanford09 contains more detail. At least one of the versions is RecursiveStateEstimation.ppt on on ScanDisk3, Kingston and on Freecom (not on laptop-C or H). The introduction.ppt from is equivalent with CS226B-02-Probabilities. The recordings are Freiburg lecture 5 part 1 and part 2
• Made an array version of the Matlab script for Exercise 2.8.1:
  Pf(1) = 0.01;
  
  for i = 2:11
  Pz(i) = Pf(i-1) + 0.333 * (1-Pf(i-1))
  Pf(i) = Pf(i-1) / Pz(i);
  end
  
• Read the documentation of matlab-prettifier.

September 3, 2017

• Reread the eighth chapter of Probabilistic Robotics, about the grid and particle filter localisation. Not only algorithms themselves, but also the adaption of the number of particles, sampling from other distributions and the rejection of subsets of the measurements based the likelyhood of an error are covered.
• In Lesson 8, Quiz 13 I tried p[i].move(0.1,5.), but Thrun wanted something different. The move was correct, but Thrun wanted a new particle set; not the moved original set :
  p2 = [] for i in range(N): p2.append(p[i].move(0.1,5.))
• Thrun wanted also a copy p = p2. Probably for the resampling step in the next Quiz.
• The next quiz is quite simple, with the provided function measurement_prob:
  for i in range(N): w.append(p[i].measurement_prob(Z))
• My first (incorrect) try was :
  for i in range(N):
  if random.random() >= w[i]:
  p3.append(p[i])
  else :
  i = i-1
  Yet, I forgot to normalize the weights.
• My second (incorrect) try was:
  W = 0
  for i in range(N):
  W = w[i] + W
  p3 = []
  for i in range(N):
  j = random.randint(0, N -1)
  while random.random() < w[j] / W:
  j = random.randint(0, N -1)
  p3.append(p[j])
  Yet, Thrun gives no explicit answer (but goes directly to the resampling wheel).The algorithm for the resampling wheel is given on page 110 (Table 4.4).
• Implemented the algorithm from page 110 with the following code, but got theresponse that my code has an orientation preference (which is strange, because Iuse the measurement_prob provided by Thrun:
  W = 0
  for i in range(N):
  W = W + w[0]
  p3 = []
  b = random.uniform(0, 1 / (N -1))
  i3 = 0
  c = w[i3] / W
  for i in range(N):
  beta = b + i / (N -1)
  while beta > c:
  i3 = i3 + 1
  c = c + w[i3] / W
  p3.append(p[i3])
  
• This is resampling based on low variance, not the wheel described in the video. The answer provided by Thrun is:
  p3 = [] index = int(random.random()*N) beta = 0.0 mw = max(w) for i in range(N): beta =+ random.random() * 2.0 * mw while beta > w[index]: beta -= w[index] index = (index + 1) % N p.append(p[index]) p = p3 which seems to be corrected with index = index + 1.
• The solution of Thrun for Quiz 22 is nicer, for sure because the orientation only settles after 10 steps:
  for t in range(T):
  myrobot = myrobot.move(0.1, 5.0)
  Z = myrobot.sense()
  It only requires a lot of indents.
• After Quiz 22, the assignment of Quiz 23 is too simple: just add print eval(myrobot, p).
• At lesson 9, I had the bycicle model directly good, although I had to peek on page 126 if I had the remembered the formula's for the center of the circle correctly:
  def move(self, motion):
  result = self
  d = motion[1]
  beta = tan(motion[0]) * d / length
  
  if beta < 0.0001:
  result.x = self.x + d * cos (self.orientation)
  result.y = self.y + d * sin (self.orientation)
  result.orientation = self.orientation + beta
  else:
  R = d / beta
  cx = self.x - sin(self.orientation) * R
  cy = self.y + cos(self.orientation) * R
  result.x = cx + sin(self.orientation + beta) * R
  result.y = cy - cos(self.orientation + beta) * R
  result.orientation = self.orientation + beta
  I print the correct values, but still the code is not accepted. Maybe I should add % (2.0 * pi)?
• The measurement function (bearing only) was simple, only had to add the modulo 2pi (and the x,y of the robot):
  def sense(self):
  Z = []
  for k in range(len(landmarks)):
  Z.append((atan2(landmarks[k][0]-self.y,landmarks[k][1]-self.x) - self.orientation) % (2 * pi))
  
• I only not added the noise model += random.gauss(0.0, self.bearing.noise)
• In the last quiz I added
  if add_noise: beta += random.gauss(0.0, self.steering_noise) d += random.gauss(0.0, self.distance_noise) but move didn't accepted a third add_noise parameter (as sense).
• The code runs, but the result ([50.31950959859751, -1.1098346990982508, 2191.270850745601]) isn't near the expected location ([x=93.476 y=75.186 orient=5.2664]). The orientation could be solved with adding an additional % (2 * pi) to motion model. That was also the conclusion of Thrun (not close enough to the groundtruth).
• No answer about the trivial resampling in the lesson 10 and 11. Maybe in the forum :-).

September 2, 2017

• Couldn't find on internet a solution for Assignment 2.8.1 (which is good).
• Found Jana Kosecka's course, who starts with Siegwart and after chapter 4 makes the transition to Thrun. Could look at her homework 7, including solution.

August 31, 2017

• In Quiz 9 of Lesson 8 they made a Python robot which can move around in a world of 100x100m with 4 landmarks. The question is to start at pose (30, 50, pi/2), turn pi/2 and move 15m, sense, turn pi/2 and move 10 meter, sense. This could be easily done with this code:
  myrobot = robot()
  myrobot.set(30.,50.,pi/2)
  myrobot.move(pi/2,15.)
  myrobot.sense()
  print myrobot
  myrobot.move(pi/2,10.)
  myrobot.sense()
  print myrobot
  
• Yet, I made a few mistakes (without myrobot = myrobot.move nothing moves, clockwise turning is negative), so the correct answer is:
  myrobot = robot()
  myrobot.set(30.,50.,pi/2)
  myrobot = myrobot.move(-pi/2,15.)
  print myrobot.sense()
  myrobot = myrobot.move(-pi/2,10.)
  print myrobot.sense()
  
• The next quiz is easy, although it is a bit acqward that you have the remember what he explicitly wants (including three noise parameters).
  myrobot.set_noise(5.0, 0.1, 5.0)
• For quiz 12 I needed to create random numbers. Tried to create the list with this code:
  N = 1000
  p = []
  for i in range(0, N):
  p[i] = robot()
  p[i].set(random.randint(0, world_size), random.randint(0, world_size), random.uniform(0, 2 * pi ))
  p[i].set_noise(5.0, 0.1, 5.0)
  
• Yet, this is not a good list initiation. This seems to be better:
  N = 1000
  p = []
  for i in range(0, N):
  p.extend( [robot()] )
  p[i].set(random.randint(0, world_size - 1), random.randint(0, world_size -1), random.uniform(0, 2 * pi ))
  p[i].set_noise(5.0, 0.1, 5.0)
  
• Actually, my code is more elegant than Thrun's solution (x = robot(); p.append(x), and my initialisation at random is not needed because this is already done in robot().

August 29, 2017

• In Quiz 26 of Lesson 4 you have to implement a multidimensional Kalman Filter based on the equation given in Quiz 25 (min 2:43). Essentially they are the equations in the book, with the matrices F and H given as a robot moving with constant speed.
• In principal the equations of Table 3.2 on page 42, only with an intermediate calculation of S, as done in the UKF (Table 3.4) and EKF-localization (Table 7.2).
• My first implementation looked liked:
  def kalman_filter(x, P):
  for n in range(len(measurements)):
  
  # measurement update
  y = measurements[n] - H * x
  S = H * P * transpose(H) + R
  K = P * transpose(H) * inverse(S)
  
  x = x + K * y
  P = (I - K * H) * P
  # prediction
  
  x = F * x + u
  P = F * P * transpose(F)
  
  return x,P
  but that fails on the first measurement update line.
• Check his multiplication and substraction overloads.
• This is my second attempt (with transpose and inverse as method of the matrix), but still fails on y-H*x
  # measurement update
  y = measurements[n]
  print y
  #y = y - H * x
  print H * x
  S = H * P * H.transpose() + R
  K = P * H.transpose() * S.inverse()
  
  #x = x + K * y
  P = (I - K * H) * P
  # prediction
  
  x = F * x + u
  P = F * P * F.transpose()
  
• Thought that the error was in the order of operations, but y = y - (H * x) still fails. The trick was to first make a matrix of the scalar: Z = matrix([[measurements[n]]]):
  # measurement update Z = matrix([[measurements[n]]])
  y = Z - H * x
  S = H * P * H.transpose() + R
  K = P * H.transpose() * S.inverse()
  
  x = x + K * y
  P = (I - K * H) * P
  # prediction
  x = F * x + u
  P = F * P * F.transpose()
  In the answer Thrun called this trick a triviality.
• Lesson 5 has again a full programming assignment, which asks to extend the 2D example to a 4D example. Thought that I had a good answer, but F * x gives can't multiply sequence by non-int of type 'float' while all elements are clearly floats:
  P = matrix([[[0.], [1000.]], [[1000.], [0.]]]) # initial uncertainty: 0 for positions x and y, 1000 for the two velocities
  F = matrix([[[1.],[0.],[0.1],[0.]], [[0.],[1.],[0.],[0.1]], [[0.],[0.],[1.],[0.]], [[0.],[0.],[0.],[1.]]]) # next state function: generalize the 2d version to 4d
  H = matrix([[[1.],[1.],[0.],[0.]]]) # measurement function: reflect the fact that we observe x and y but not the two velocities
  R = matrix([[[0.1],[0.]], [[0.],[0.1]]]) # measurement uncertainty: use 2x2 matrix with 0.1 as main diagonal
  I = matrix([[[1.],[0.],[0.],[0.]], [[0.],[1.],[0.],[0.]], [[0.],[0.],[1.],[0.]], [[0.],[0.],[0.],[1.]]])# 4d identity matrix
  
• Looked at the answer and the main difference is in the P (4x4) and H (4x4). Also, F is defined with 4 vectors (two brackets, instead of three brackets).
• If I correct F, then the program fails on my 2x2 P (which should be 4x4):
  F = matrix([[1.,0.,0.1,0.], [0.,1.,0.,0.1], [0.,0.,1.,0.], [0.,0.,0.,1.]])
• Made the 4x4 P and H, but now get warning that y = Z.transpose() - (H * x) is a substraction of non-equal dimensions.
  P = matrix([[0.,0.,0.,0.], [0.,0.,0.,0.], [0.,0.,1000.,0.], [0.,0.,0.,1000.]]) # initial uncertainty: 0 for positions x and y, 1000 for the two velocities
  #P.show()
  F = matrix([[1.,0.,0.1,0.], [0.,1.,0.,0.1], [0.,0.,1.,0.], [0.,0.,0.,1.]]) # next state function: generalize the 2d version to 4d
  #.show()
  #x = F * x
  ##.show()
  H = matrix([[1.,0.,0.,0.], [0.,1.,0.,0.]])
  #measurement function: reflect the fact that we observe x and y but not the two velocities
  H.show()
  R = matrix([[0.1,0.], [0.,0.1]]) # measurement uncertainty: use 2x2 matrix with 0.1 as main diagonal
  #R.show()
  I = matrix([[1.,0.,0.,0.], [0.,1.,0.,0.], [0.,0.,1.,0.], [0.,0.,0.,1.]])# 4d identity matrix
  
• So, my error was H, this is a 4x2 matrix:
  H = matrix([[1.,0.,0.,0.], [0.,1.,0.,0.]])

August 27, 2017

• Continue with Lesson 4: The application of Bayes rule in the measurement update could be found for example on page 103 (Mathematical derivation of Particle Filter). Thrun claims that the rule for combining the mean and variance is essential for the measurement step of the Kalman Filter (which is logical because it is the product of two Gaussians), yet I also didn't see this two formula's directly applied. On pag 42 in Table 4.1 (Kalman Filter) you can recognize the two predict formulas
  def predict(mean1, var1, mean2, var2):
  new_mean =mean1+mean2
  new_var =var1+var2
  return [new_mean, new_var]
  in line 2 and 3, but the update is well hidden in the Kalman gain.
• Yet, the Kalman filter as a single combination of a predict and update is nice (and in line with the previous lessons (discrete Bayesian Filter):
  for k in range(len(measurements)):
  [mu, sig] = update(mu,sig,measurements[k],measurement_sig)
  [mu, sig] = predict(mu,sig,motion[k],motion_sig)
  

August 26, 2017

• Reread the seventh chapter of Probabilistic Robotics, about the localisation with EKF and UKF. The example is with the Aibo robots; it will still be a great assignment to this with Nao robots.
• The introduction of Lession 4 could be used to introduce Sebastian Thrun (nice, short, clear about Stanford and Google car.
• Lesson 4 starts at unbelievable low level. Quiz 3-5 can be skipped.
• Programming starts in Quiz 7. Quiz 8 finally starts with localisation. Had some difficulty with Quiz 9. Motion update has a integral, so uses the rule of total probability. Both have conditional probabilities, but only the measurement update is according to Thrun derived from Bayes rule.
• Finally, Quiz 16 has a real Python question. Couldn't directly find the Gaussian multiplication rules in the book, as given in quiz 12: m_new = (s_2 m_1 + s_1 m_2)/(s_1 + s_2) and s_new = 1 / (1/s_1 + 1/s_2). In first instance I had the weighting of the mean wrong, but by checking the answer found the mistake fast enough:
  def update(mean1, var1, mean2, var2):
  new_mean = (mean1 * var2 + mean2 * var1) / (var1 + var2)
  new_var = 1 / (1./var1 + 1./var2)
  return [new_mean, new_var]
  
• Finally, in Lesson 21 the step to Multivariate Gaussians is made. Thrun conveses that he also has to look up the formula of a Multivariate Gaussian :-)

August 22, 2017

• Reread the sixth chapter of Probabilistic Robotics, about the sensor model. A lot of attention for the likelihood field, although that never became very popular.
• Made a miscalculation in Quiz 3 of Lesson 2: I calculated P(B|F)P(F) as 1 x 0.001, instead of 0.9 x 0.001. It is a bit confusing that Thrun used the same symbol hat for non-normalized and not true. I calculated the normalizer correctly.
• Quiz 4 contains some nice exam questions.
• Started Quiz 5 with a simpler test:
  colors = [['G','G','G'],
  ['G','R','G'],
  ['G','G','G']]
  measurements = ['R']
  motions = [[0,0]]
  
• Although maybe not pritty, this sense functions seems to work:
  def sense(p, measurement,sensor_right):
  # calculates the normalized probability of sensing the 'measurement'
  for col in range(len(colors)):
  for row in range(len(colors[0])):
  hit = (measurement == colors[col][row])
  p[col][row] = p[col][row] * (hit * sensor_right + (1-hit) * (1-sensor_right))
  #calculate sum for normalization
  s = 0
  for col in range(len(colors)):
  s = s + sum(p[col])
  #normalize
  for col in range(len(colors)):
  for row in range(len(colors[0])):
  p[col][row] = p[col][row] / s
  return p
  
• Tested it for second case:
  colors = [['G','G','G'],
  ['G','R','R'],
  ['G','G','G']] and it gave the correct:
  [[0.00000,0.00000,0.00000],
  [0.00000,0.50000,0.50000],
  [0.00000,0.00000,0.00000]]
• For sense_right of 0.8 it gave:
  [[0.06667,0.06667,0.06667],
  [0.06667,0.26667,0.26667],
  [0.06667,0.06667,0.06667]] As indicated at minute 3:04 of the question.
• Also implemented the function move:
  def move(prior,motion,p_move):
  # calculates the normalized probability of moving a chessboard 'motion'
  p = prior
  for col in range(len(colors)):
  for row in range(len(colors[0])):
  p[col][row] = prior[(col-motion[0]) % len(colors)][(row-motion[1]) % len(colors[0])]
  #calculate sum for normalization
  s = 0
  for col in range(len(colors)):
  s = s + sum(p[col])
  #normalize
  for col in range(len(colors)):
  for row in range(len(colors[0])):
  p[col][row] = p[col][row] / s
  return p Which worked fine for motions = [[0,0]].
• Also tested it for motions = [[0,0],[0,1]], combined with measurements = ['R','R'] (as indicated in minute 3:30 of the Quiz). Actually, the additonal test were indicated below the movie!. Yet, my answer is wrong, so I made a left / right or up /down error. My answer is:
  [[0.02564,0.02564,0.02564],
  [0.02564,0.41026,0.41026],
  [0.02564,0.02564,0.02564]] instead of
  [[0.03333333333, 0.03333333333, 0.03333333333],
  [0.13333333333, 0.13333333333, 0.53333333333],
  [0.03333333333, 0.03333333333, 0.03333333333]]
• Also added the change on the determinstic move p_move: for col in range(len(colors)): for row in range(len(colors[0])): p[col][row] = p_move * (prior[(col-motion[1]) % len(colors)][(row-motion[0]) % len(colors[0])]) p[col][row] = p[col][row] + (1-p_move) * prior[col][row]
• Found the error, I first made two moves, than two measurements. Correct call is : for i in range(len(motions)):
  pm = move(p,motions[i],p_move)
  p = sense(pm,measurements[i],sensor_right)
• Checked it against to answer of Thrun, but seems the same. Will try it with his implementation (minute 3:48).
• The error was in the instantation with a copy aux = p instead of aux = [[1.0 for row in range(len(colors[0]))] for col in range(len(colors))] (or 0.0 as Thrun suggested).
• The Delft Workshop is on October 2th.

August 17, 2017

• Reread the fifth chapter of Probabilistic Robotics, about the motion model. Dieter Fox example of the motion model of the drone is a good example of more sophisticated motion models.
• At page 124: a normal distribution can be approximated by 12 draws from a uniform distribution. Why 12x? Check Winkler 1995, p. 293?
• Thrun's AI for Robotics course has 23 lessons. Only a subset is about theory: Localization, Kalman Filters, Particle Filters, Search, PID Control and SLAM. The course ends with a project: Runaway robot.
• Had to look up the elif construction from Python, after that my solution for Quiz17 was correct:
  def move(p, U):
  q=[]
  for i in range(len(p)):
  if i-U < 0:
  q.append(p[i-U+len(p)])
  elif i-U >= len(p):
  q.append(p[i-U-(len(p)+1)])
  else:
  q.append(p[i-U])
  return q Thrun's solution was more elegant, with the use of modulo q.append(p[(i-U) % len(p)]) and as alternate solution even without loop with the slicing operator (q = p[-U:] + p[:-U]).
• Quiz 20 seems very simple, but as indicated: this is convolution!
• Quiz 25 is maybe a good starting point for Blended learning

August 16, 2017

• Reread the fourth chapter of Probabilistic Robotics.
• On page 102, they indicate that resampling an interval is related with the mathematical concept of Borel sets, which is used to integrate measures from a probability.
• Looked at Dieter Fox's University of Washington CSE 571: Probabilistic Robotics (2016). Dieter adds a lecture about Gaussian Process (from Rasmussen) and covers the book until Chapter 14. Yet, the second half a covers subjects like RGBD mapping, Manipulation Planning and Deep Learning.
• Liked slide 3-5 and 10 and slides.
• Next to the book, he also adds some articles, like Octomaps and KinectFusion.
• Dieter has three homework assignments. Parly written, partly programming assignments. The first programming assignments are to implement kernel functions and to learn the dynamics of a cartpole.
• The second homework is implementing localization with a Kalman Filter and a Particle Filter.
• The third homework is pathplanning (A* and Random Tree). All assignments in Matlab.
• Homework is only half of the grade, the other half is an open project.
• Another relevant course is Robotics Learning from Daniel Lee. Price is $34 after 7 days trail. It is a 4 week course, with as last two assignments Kalman Filter and Particle Filter localization (in Matlab).
• Sebastian Thrun latest course seems to be Artificial Intelligence for Robotics.
• Knowledge Clip 3 (Total Probability) explains Figure 1.1.
• Was able to make the Generalized Uniform Distribution quiz (p = [0.2,0.2,0.2,0.2,0.2]). Adding print(sys.version) showed that Udacity uses Python 2.7.6 and GCC 4.8.4. Giving a C-program to the interface gave an error in self.code = compile(source, self.filename, 'exec').
• My Matlab version on nb-ros is R2015a. The syntax is nearly the same (p = [0.2,0.2,0.2,0.2,0.2];), except how to call one of the elements (p[1] vs p(1)).
• For Quiz10 I already need numpy. With Matlab this is simple (pz .* p), with numpy (p = np.multiply(pz, p)), yet I couldn't submit a solution with an numpy-import. Used the suggestion of stackoverflow and solved it with:
  p = []
  for i in range(0, len(prior)):
  p.append(prior[i]*pz[i])
  
• Solved Quiz11 also with a loop, but now I could have used the function sum: t = 0
  for i in range(0, len(p)):
  t = t + p[i]
  
  print t
  
• My solution for Quiz12 was nearly Thrun's solution, but instead of an if-statement he used an extra variable hit = (Z == world[i]) ; q.append(p[i] * (hit * pHit + (1-hit) * pMiss):
  q = []
  for i in range(0, len(world)):
  if Z == world[i]:
  q.append(pHit*p[i])
  else:
  q.append(pMiss*p[i])
  return q
• For Quiz13 I used another variable, but Thrun reused the variable q with q[i]=q[i]/s, while I made a new list:
  t = sum(qu)
  q=[]
  for i in range(len(qu)):
  q.append(qu[i] / t)
  return q
• For Quiz14 I reused the index i, while Thrun used a new one:
  for i in range(len(measurements)):
  p = sense(p,measurements[i])
• Stopped at implementing the move function (Quiz17).
• Looked if I could reproduce the Quizes in Matlab. Unfortunelly, strings arrays can be used in Matlab R2017a, before you have to use cell arrays of strings:
  • run_sense.m
  • sense.m

August 14, 2017

• Followed the instructions of the 2nd year BscKIi and installed wget http://sbt.science.uva.nl/boydki_scripts/BscKIYear2.sh. This installed Weka and numpy,nltk,matplotlib,pillow for python2.7 and python3.

August 13, 2017

• Reread the third chapter of Probabilistic Robotics.

August 9, 2017

• Reread the first two chapters of Probabilistic Robotics.
• The exercises from Freiburg are now in Python. Should try Sheet 1. Solutions are given.
• The exercises from Dieter Fox are still in Matlab: Gaussian Processes.
• The Robot Mapping exercises from Freiburg are also still in Octave: Odometry.
• The Robot Mapping course links to the Handbook of Robotics. In the 2nd edition the relevant chapters are 45 (World Modelling - page 1136-1152) and 46 (SLAM - page 1154-1175).
• Another relevant chapter is Christensen chapter 5 (Sensing and Estimation - 91-112 - incomplete save). For an introduction chapter, this goes quite deep, with derivation of least-square estimation, Kalman filters and Conditional Random Fields.

May 1, 2017

• Printed Visual place recognition: A survey from Paul Newman.

March 15, 2017

• Started reading Graph-Based Simultaneous Localization and Mapping: Computational Complexity Reduction on a Multicore Heterogeneous Architecture, which splitted the backend of GraphSlam into 7 functional blocks and performing the computations on a Nvidia Tegra K1.
• Another usefull resource could be Nardi's Introducing SLAMBench, a performance and accuracy benchmarking methodology for SLAM, which allows evaluation of different implementations of the KinectFusion algorithm.

Previous Labbooks

• Labbook2015.html
• Labbook2012.html
• Labbook2011.html
• Labbook2010.html
• Labbook2009.html
• Labbook2008.html
• Labbook2007.html