Kalman filtering / motor physics

As part of my board / motor work I have the need for modeling some measurements w.r.t. motor physics, most notably system variable estimation under systemic noise. I am developing a 1D Kalman filter for that, is anyone interested in adding it to the library? I see all the time people complaining about “jerky signals”, e.g. noisy measurements. My aim it to make it really small, fast and simple to use to allow for quick motor physics in a feedback loop (I am using it for a fast servo with a gain of about 1:10 which really amplifies the noise).

I really suck as Git, so I could post the code and a nice person knowledgeable about Git and libraries and proper code maintenance could add it to the simpleFOC library? If I do it right the 1D Kalman should take perhaps 20 lines of code and a single function call.


Edit: Perhaps the 1D Kalman could be added to the angular sensor reads as an option to “smooth” the signal instead of using averages. Using averages is quick and dirty but I have found it does create some feedback issues with high frequency signals.


Having a choice of filters would be nice. I believe it would require some significant refactoring, though (as the simple LPF seems quite hard-wired right now).

1 Like

Hey Valentine,
I’d be happy to integrate your work to the library code.

Kalman filter is a good idea, it would be considerably more difficult to tune and find the parameters but once you have the performance would be much better.
Maybe it would make sense to use it for the motor parameter estimation and similar stuff, but I’d be in to discuss it and integrate it in a form that suites the community.

@quentin you’re right that the filter is pretty much imprinted into the library at the moment. But it is very generic and for the most of the regular moving average filters that you’d implement usually you can express them as low pass filter.
So having a generic filter implementation would be interesting, and if there are people interested it could be an addition to the library, I’d be in. :smiley:

I’m always in to add some more control engineering :smiley:


One-dimensional Kalman Filtering for simple, well behaved time series
physical measurements such as angle, temperature, velocity, etc.
This algorithm works by a two-phase process. For the prediction phase, the Kalman filter produces estimates
of the current state variables, along with their uncertainties. Once the outcome of the next measurement
(necessarily corrupted with some error, including random noise) is observed, these estimates are updated
using a weighted average, with more weight being given to estimates with greater certainty.
The algorithm is recursive. It can operate in real time, using only the present input measurements and
the state calculated previously and its uncertainty matrix; no additional past information is required.
The predicted value could be fine-tuned by modifying the error and system noise inputs real-time to achieve optimal results.
This however will require extending the class to monitor error and noise real-time and is a very complex topic.
Fun fact, the first publicly known application was made at NASA Ames Research Center in the early 1960s
during feasibility studies for circumlinear navigation and control of the Apollo space capsule.
The algorithm was implemented in assembler in magnetic core memory on the capsule board navigation module.
Some decent default values: measurement_error = 2; estimation_error = 2; system_noise = 0.01;

include <KalmanFilter.h>
kalmanFilter KalmanFilter;
float measurement = kalmanFilter.getEstimate(measurement, 2.0, 2.0, 0.01)

    class KalmanFilter
      float getEstimate(float signal_measurement, float measurement_error, float estimation_error, float system_noise);
      float previous_estimate = 0.0;
      float gain = 0.0;
      float signal_estimate = 0.0;
      float error_estimate = 0.0;


    float KalmanFilter::getEstimate(float signal_measurement, float measurement_error, float estimation_error, float system_noise)
      if ( error_estimate == 0.0 ) error_estimate = measurement_error;
      gain = error_estimate/(error_estimate + measurement_error);
      signal_estimate = previous_estimate + gain * (signal_measurement - previous_estimate);
      error_estimate =  (1.0 - gain)*error_estimate + fabs(previous_estimate-signal_estimate)*system_noise;
      return signal_estimate;

Please feel free to modify as needed. I did it a bit dirty to take care of the init instead, with the cheat:

if ( error_estimate == 0.0 ) error_estimate = measurement_error;

because I didn’t want to initialize first with default values, and wanted to be able to change the parameters real time, however checking for 0.0 is a bit dangerous.

You can extend


by creating some body and logic, to handle initialization params, however this will increase the memory footprint.

As they say, caveat emptor.

Edit: I am using it to design a high-torque (many Kg-m) natural movement servo joint, because when you have really heavy payloads the dynamic stress will exceed the bearing capacity, and you need to implement some good filtering coupled with S-curves. The Kalman filter on the target angle does that extremely fast and you can modify the parameters to achieve whatever curve and frequency/error filtering you want with virtually zero memory footprint.