Localisation ends up with cluster outside of the boundaries

[dannystaple]

Doug has seen the localisation code run, as the robot moved, although the cluster begins to group, it does so outside the boundaries.

[dblanding]

The picture shown above was taken about a minute after the robot stopped moving. As I recall, the particles stayed within the boundaries while the robot was moving, but then went beyond the boundaries after I clicked on the stop moving button.

The picture below was taken while the robot was still moving, roughly in the middle of the arena and moving to the left. I thought it was odd, because I thought the particles should be located roughly where the robot was located, and pointing in the same direction that the robot was moving.

[dblanding]

I was inspired to revisit the robot's localization performance, now that I have the 64-bit Full Desktop version of the OS. It definitely behaves differently than it did before. Initially, the robot takes off at a spirited pace and the field of possible poses respond encouragingly, but then after a couple of seconds, the robot just stops. I was wondering if you have had similar results.

[dannystaple]

Could this also have been stopping due to the voice agent service?

[dblanding]

Actually, my distance sensor doesn't work. Now that I think about it, I seem to recall getting it to work briefly but then stop after a few seconds. But now it doesn't even do that. When I first power up the robot and ask what services are running, I get:

doug@raspi4:~ $ systemctl list-units --all --no-pager $(systemctl show -p Before mosquitto.service --value)
  UNIT                            LOAD   ACTIVE   SUB     DESCRIPTION            
  config_store.service            loaded active   running config_store
  distance_sensor_service.service loaded active   running distance_sensor_service
  inventor_hat_service.service    loaded active   running inventor_hat_service
  launcher_service.service        loaded active   running launcher_service
  voice_agent.service             loaded active   running voice_agent
  web_server.service              loaded active   running web_server
  wheel_control_service.service   loaded active   running wheel_control_service  
  multi-user.target               loaded active   active  Multi-User System
  shutdown.target                 loaded inactive dead    System Shutdown

Then I set the robot in the arena and use the web interface to start loclalisation and start moving:

doug@raspi4:~ $ systemctl list-units --all --no-pager $(systemctl show -p Before mosquitto.service --value)
  UNIT                            LOAD   ACTIVE   SUB     DESCRIPTION            
  config_store.service            loaded active   running config_store
  distance_sensor_service.service loaded active   running distance_sensor_service
  fixed_distance_avoider.service  loaded active   running fixed_distance_avoider
  inventor_hat_service.service    loaded active   running inventor_hat_service
  launcher_service.service        loaded active   running launcher_service
  localisation.service            loaded active   running localisation
  voice_agent.service             loaded active   running voice_agent
  web_server.service              loaded active   running web_server
  wheel_control_service.service   loaded active   running wheel_control_service  
  multi-user.target               loaded active   active  Multi-User System
  shutdown.target                 loaded inactive dead    System Shutdown

I get the screen showing a bunch of poses overlaying the boundaries of the arena, but the robot doesn't move.

When I use the web interface to see if the distance plotter works, it doesn't work. Just shows all black squares.

[dannystaple]

Please let me know how this goes with the now updated Chapter 12.

[dblanding]

The robot is staying within the boundaries of the arena, but I have noticed a couple of other problems, which I list below.

1. Once the poses start to converge, they don't agree with the physical location of the robot in the arena.
2. The 2nd problem is the robot doesn't always reliably start moving when the Start moving button is clicked. As I was making a video to show both the physical robot (in the arena) and the screen image of the poses, I had to make numerous attempts, restarting the robot each time, before I succeeded in actually getting the robot to start moving when I clicked the Start moving button.

Unfortunately, the video was too big (31.9MB) to upload here, so I show a screenshot below that shows problem 1, just before the robot spontaneously stops moving (problem 2).

Regarding the 2nd problem, one clue is that the green pilot light on the Inventor Hat normally turns off immediately when I press the shutdown button (or click the button on the web interface) but when I shut it down to give it a fresh start after failing to perform the localisation behavior, the pilot light stays on until the Pi completes its shutdown.

[dannystaple]

Hmm, problem 2 sounds like the pi is losing contact with the inventor hat board. Are you able to get any errors from the service in journal ctl there?

With problem 1, I can try your geometry in the simulator, and see how things behave. It would be useful to have the geometry of it with as little rounding as possible.
Are the orientation changes registering properly?

[dannystaple]

I also have another pretty good theory on what may be a problem - the wheel camber can mean that the robots wheels do not match the measured wheel base distance, and that the contact points are on the inside edge of the wheels, nothing like the whole tyre measurements suggested. I'm trying the inner measurements assuming the wheel camber puts a thin edge in contact with the floor, and getting more accurate turns when recalibrating turns, which may lead to a more accurate movement model.

[dblanding]

Sorry I haven't had a chance to look at this further. But in answer to your question about the changes in pose angle, Yes they look to be moving in the correct direction and roughly in the right amount of rotation.
That's an interesting observation. There is most definitely some camber.

[dannystaple]

I have reproduced this - and find that I can easily get outside results. The MCL model probably needs to be more tolerant - the first issue being that the binary nature of the current weighting model means probabilities collapse really quickly.
The simulation - it runs perfectly, but then I neeed to introduce some real world factors to see where it becomes problematic.
Things that seem to be contributing factors:

• wheel camber as noted.
• Wheel diameter mismatch
• Any shear/distortion in the arena - I'm going to add something to make it less binary so it may cope - a "crosshairs" system, we can try this together, once it looks to solve things here.
• Wheel slipping - I will try to model this in the simulation.
• Latency - there could be system latency that makes it unable to respond - also something I can model.

[dblanding]

The XRP robot gets around this issue by using O-ring tires.

[dannystaple]

I've simulated a tiny (1/1000) shear factor in the simulator. I know my shop skills, in foam, are not going to be perfect, so right angles might be nearly right angles. That tiny shear factor easily makes it produce poses outside. So I'm trying an experiment with a more "tolerant" boundary model, using a 5 tap crosshair, and taking the mean across the 5 hits for each pose. This should make a more "gradual" sample for the boundary model, since the binary one can make the probabilities collapse to quickly per pose.
I'll report back when I've built that, to see if it makes the model more tolerant. The problem being the current model doesn't have much tolerance for shear, bowing or minor scaling at all it seems - which is going to be unrealistic.

[dannystaple]

Well, I added a shear factor on both axes to the simulation. The model was resilient to this, even when visually quite distorted.
I can say with some certainty that this is not the problem.
The wheelbase mismatch is the easiest way to reproduce this behavior. But I want to eliminate other factors first too.

The crosshair thing, makes it more quickly resolve when there's shear, but the model copes without it too. Yet to see what it does with wheel camber and mismatching base.