Replay bicycle-filtered trajectories

[Atcold]

@bo-rc, I'm trying to use your filter. Can you see if this is correct? The vehicles do not move.

[bo-rc]

@Atcold I currently are occupied by tasks, I will look into this later.

Meanwhile, the collision branch has some experimental code that I used to test-run a Ukf. However, that code is not for production purposes so please do optimize the way we use a Ukf.

Note that Ukf needs some time to converge its internal states, so the initial several seconds the vehicle won't move so nicely. We need a way to address this too.

[Atcold]

Okay. I think that I am misusing your code. I'm looking into your test_ukf_bicycle.py script. The plot looks good (even though there are no labels).

[bo-rc]

yes, you can refer to test_ukf_bicycle.py and start from there. One thing I wanted to try but didn't have time is to extract trajectories of some bad-behaving vehicles and use the script to plot the filtered path and vehicle orientations. I think it will help us understand what is wrong and what needs to be done.

Another thing I wanted to try is that we do a low-pass filtering to the raw trajectory and then use Ukf in the simulator. Maybe you can have a try!

[Atcold]

Yeah, just got the trajectory replay (tracking) to work. Now, I think we need a stronger prior over the internal predicted state (just set noise = 1e-4). Then we have to extract the actions, with which we'll update the car's state (and not via the measurement).

The bug was that car.ukf.x != car.ukf.ukf.x 😡