3

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:

preliminary library design

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?
  • 3
    Graduate students have completed thesises that are less work than what you're asking for. – whatsisname Nov 29 '16 at 14:32
  • You need a math-first approach. You need to capture some real recordings of the data from each sensor, while the device travel along a known trajectory. Then you analyze the data and decide what strategies you need. Then you investigate how data needs to be passed between the different parts of the software, and finally you design your software parts to enable that. It is very likely that other people have researched and implemented things like that; by reading their reports you may be able to learn the proper way. Unfortunately not many users on this site have expertise to answer this. – rwong Nov 29 '16 at 16:23
  • I am not worried about maths. In fact I have already programmed a Kalman filter for orientation estimation of an IMU. The design of this library is part of my PhD. We do not need optimization when dealing with the GPS, which gives us measurements at a rate of 10Hz at most. When working with IMUs we observe a strong dependence of estimation accuracy with the sampling rate. That is why I am looking for optimization. I have what I consider decent maths skills. But my programming skills have deficiencies. That is why I am asking for some advice in the library design, and in how to optimize. – pablo bernal Nov 29 '16 at 21:29
  • Perhaps your IMU class should just act as a caching array of IMU sensor readings sampled at 1000Hz, or at some interrupt frequency (which is unfortunately unlikely to match the ideal sampling frequency of the IMU sensor), and do processing later. – rwong Nov 29 '16 at 22:00
  • In other words I guess you want to perform integration (integral over time) of IMU sensor readings? And with respect to rate change (derivative) of IMU readings, you will need some PID (proportional integral derivative) smoothing of them? – rwong Nov 29 '16 at 22:01
3

Recall that a general Kalman filter requires several matrix multiplications and additions, and at least one matrix inversion, on every predict/update cycle.

Even if you get down to a scalar filter, where the matrix inversion turns into a simple division, you're still doing quite a bit of math.

Don't worry too much about the efficiency of accessing your state vector from outside your estimator. Unless you pick a really brain-dead way of doing the access, the Kalman filter updates will probably dominate your timeline.

2

[H]ow can I have direct access to variables of State from ''update()'' methods of InformationSources?

This is almost certainly a case of premature optimization.

It's not clear what your actual performance requirements are (you should think about defining these), but it's highly unlikely that the overhead of some method calls is going to make any difference to whether you meet those requirements.

You should implement this as normal, with methods to get the information you need.

In the unlikely event that this does become a performance issue, you can rewrite it to work another way (a performance requirement would be a reasonable justification for breaking the normal "good practice" rule of not accessing class variables directly. So I would suggest your proposed methods are fine, should you need a solution).

Your Answer

By clicking “Post Your Answer”, you agree to our terms of service, privacy policy and cookie policy

Not the answer you're looking for? Browse other questions tagged or ask your own question.