Rewrite locationd in C++

[adeebshihadeh]

No description provided.

[wolterhv]

Hello @adeebshihadeh, you can assign me to this issue.

[wolterhv]

Took some time today to make a graph of the work to be done. Here is the key:

• Red modules are in python and need to be written in cpp.
• Yellow modules may be readily available as third-party cpp libs
• Green modules are already available in cpp

The common Params module seems to be partially available in cpp, I think only the put function is not in the cpp version.

[wolterhv]

If anyone wants to track progress of this, they can see https://github.com/wolterhv/openpilot/tree/issue2386-wips

[pd0wm]

We don't need to move the code generation to C++. That's fine in python/sympy.

Currently a lot of time is spend in the kalman update and predict functions. Only those have to be rewritten in C++, and can be wrapped in Cython to keep as much of locationd in python as possible.

Before we can do any of the actual rewriting we need some integration tests around the Kalman filter to make sure the results don't change.

[wolterhv]

Thanks for the input Willem, I'll focus on those functions when I get back to it. I noticed after a while that some of the modules were for code generation, but I haven't updated the diagram. Thanks for the heads-up on that too.

[wolterhv]

I haven't set aside much time for this until this morning. If you can take a look at this commit and let me know whether I'm on the right track from your perspective, that would be very helpful. At the moment, my code is heavily commented with reminders and pointers while I get more comfortable with the codebase. I will happily remove them before an eventual PR.

(Edit: There should be at least some arguments in the testing functions which don't make sense, I plan to fix those as I get a better understanding of the context.)

I basically found that rewriting the Kalman update and predict functions basically boils down to rewriting the predict_and_observe* functions and the get_R function, from the LiveKalman class. Since the predict_and_update_batch function belongs to the rednose module and calls some C++ code already, I'm not going to touch it unless there's explicit interest in that.

[pd0wm]

We already rewrote the bare minimum to support locationd and paramsd: commaai/rednose#9. We will now rewrite the actual service in C++ to achieve the final speedup.

However, we did not implement the stuff needed to run https://github.com/commaai/openpilot/blob/master/selfdrive/locationd/models/loc_kf.py with full raw GPS measurements. This requires porting augment, get_augment_times, rts_smooth and maha_test (https://github.com/commaai/rednose/blob/master/rednose/helpers/ekf_sym_pyx.pyx#L180). If you're looking for something to work on you can make a PR to rednose with support for these functions on the C++ side. You can test against the python implementation similar to how we test the other functions.

If you're working on this stuff it's best to open a draft PR to the appropriate repository as soon as possible. Otherwise we can't see what you're working and we risk doing duplicate work.

[wolterhv]

Sure, I'll give those a shot and open a draft PR. Thanks for the header!