I want to write a library for Mechanical State Estimation of a vehicle. This is, estimate variables as position, velocity an so on, using the information provided by different sensor measurements (GPS, IMU, ... ).
These are things to keep in mind:
- I am not a very skilled programmer.
- The estimation algorithm will be the Kalman filter.
- The library is intended to be used in real-time systems.
- I want to write the library in both, C++ and Java.
- I think that an object-oriented design could help, but what I really want is the code to be efficient (fast updates, given that we could want it to run in microcontrollers, such as the ones in Arduino, or Raspberry Pi. There are IMUs able to supply data at 1000Hz, but the fastest update rate I have reached with Arduino is about 50Hz).
I have some considerations that the implementation should fulfill:
- The estimated state must be predicted using our knowledge about the process.
- The estimated state must be updated fusing the information of a sensor set.
- We could be interested in different state estimations, each constructed from different information (from different sensor sets, from measurements taken at different frequency, ... ).
- Each sensor gives measurements that are differently related with the state of the system, so each sensor should have a different ''update()'' method.
- Each sensor has different position, orientation, calibrations, and characteristics.
- I would like to be able to expand the library adding sensor types, or variables to the state in an easy way.
I've outlined a preliminary design for the library:
Finally, the questions are:
- Knowing that I want as fast access as posible, ¿how can I have direct access to variables of State from ''update()'' methods of InformationSources? I have read that for Java I could define the 2 classes in the same package, and define the variables as protected. I also know about the ''friend'' keyword in C++. But I do not know if these are good practices.
- I would appreciate any other possible ideas, so, ¿do you devise some other design?