Thanks to tinympc-rs[https://github.com/peterkrull/tinympc-rs]
Rust port of TinyMPC: https://github.com/TinyMPC