Smoothing noisy sensor data in real time

[svdsinner]

I'm working on a robotics project that has a sensor that ultimately returns a Vector3 direction that points to another object that is moving. The sensor readings*** can be assumed to be a standard gaussian distribution of mildly noisy data that is centered on the correct reading, with occasional missing readings (the sensor will occasionally say it doesn't know the direction).

I'm looking for known techniques that that can do a few things in real-time***:

1. De-noise the sensor output. The other object is moving smoothly, so the Vector3 direction should move smoothly as well. I'd like to find a smart way to smooth the "current direction" of the most recent sensor reading (30 sensor reading/sec) to be more reliable than basing my calculations off of a non-smooth noisy raw direction that the sensor puts out.
2. Determine the angular velocity of the change in direction of the sensor reading (IOW, the derivative of the sensor reading), which would require the smoothed current direction and a smoothed point half a second in the past. The past point obviously can be smoothed more reliably than the since it has both past and future datapoints to smooth against.
3. Determine the angular acceleration of the change in direction of the sensor reading (IOW the 2nd derivative of the sensor reading), which I assume would require an additional smoothed reading from a second ago so I can difference the two angular velocities. Or maybe there is a better mathematical way to get the angular acceleration?

I'm fairly sure that this is a solved problem and I just need to figure out the names of the techniques I should be using for smoothing the Vector3 direction outputs, and possibly any better techniques for getting the velocity and acceleration of the change in direction of those vectors than just picking fixed times in that past to use to determine the changes with basic vector math.

*** The sensor outputs a reading every 33ms. The faster the three values (current direction, derivative of the direction and 2nd derivative of the direction) can be computed, the better, so fast and "pretty close to correct" is far superior to slow and perfect. The algorithm' calculations will be running on a dedicated core of a modern SBC, while other cores are working on their own dedicated calculations. Since the memory bus is shared between all the cores, keeping the working set of data small is beneficial.

 

[Mark086]

Kalman filter?
https://en.m.wikipedia.org/wiki/Kalman_filter

There may be others, kalman filters are used for gps sensor data integration, (position, Doppler, motion)

 

[OrangeCream]

Edit: Mark086 has the better answer, though I don’t know how to implement it myself. Google does show some implementations in Python:
https://www.geeksforgeeks.org/kalman-filter-in-python/

I don’t know what languages you’re using but there are numeric libraries that manage statistical average and standard deviations. If you use a discounted average algorithm you can then discard any signals that are, for example, three standard deviations away from the average. If the signal is more than 3 standard deviations away then you can use the average.

 

[hanser]

Huh, I just told Claude to implement a Kalman Filter in C# using Unity's Vector3 data type as the input, and it emitted something strangely plausible.

 

[svdsinner]

Kalman filter?
https://en.m.wikipedia.org/wiki/Kalman_filter

There may be others, kalman filters are used for gps sensor data integration, (position, Doppler, motion)

You could be right. I will start reading up on them. My knowledge of Kalman filters is limited, and I have no idea if they apply to Vector3 data. I do remember the Ardupilot source code uses them a bunch for their sensors. I might be able to find some examples of how to use them with Vector3 data.

 

[svdsinner]

Huh, I just told Claude to implement a Kalman Filter in C# using Unity's Vector3 data type as the input, and it emitted something strangely plausible.

That's the fun of AI generated code. I figured out how to use Unity by asking CoPilot to give me examples of how to do whatever Vector3 stuff I needed to accomplish. In true AI fashion, >80% of the answers were good answers. For the rest it was a blend of random misunderstanding of what I was asking, code with easy to miss edge-case bugs and code that called methods on Unity object that don't exist on those Unity objects.

But, I will definitely see what I can get generated and see how well it appears to validate against reality.

 

[MilleniX]

Kalman filters are generally the initial go-to method for dynamic state estimation problems. To guide you a little bit, I think you'll want to make sure that the state you're estimating is the position and motion of the target object (relative to your robot is fine - cf Galilean Relativity), with the consequent sensor motion parameters (i.e. "where will I see if next?") being calculated from that. You don't want to end up calculating on weird non-linear dynamics of doing everything in a spherical or cylindrical coordinate frame if you don't have to.

Also, if any of your sensor parameters are modular (i.e. angle around the platform), be careful about wrap-arounds.

 

[hanser]

That's the fun of AI generated code. I figured out how to use Unity by asking CoPilot to give me examples of how to do whatever Vector3 stuff I needed to accomplish. In true AI fashion, >80% of the answers were good answers. For the rest it was a blend of random misunderstanding of what I was asking, code with easy to miss edge-case bugs and code that called methods on Unity object that don't exist on those Unity objects.

But, I will definitely see what I can get generated and see how well it appears to validate against reality.

Yeah I feel like it gets me to the right county, but the rest is up to me, especially with algorithmic stuff like this.

 

[svdsinner]

. . . you'll want to make sure that the state you're estimating is the position and motion of the target object . . .

The only thing I will have is the direction to the other robot with no range information, so I'm going to smooth the direction by simply assuming it should be smooth since the other object will be moving smoothly and hoping I don't run into the gimbal lock problem.
The fun stuff happens as I try to figure out a model to get an idea of where the other robot is and what direction it is moving from nothing but that and the dead-reckoning position of where the sensor is.

You don't want to end up calculating on weird non-linear dynamics of doing everything in a spherical or cylindrical coordinate frame if you don't have to.

Also, if any of your sensor parameters are modular (i.e. angle around the platform), be careful about wrap-arounds.

I think you just summed up why this has the potential to be challenging.

 

[ShuggyCoUk]

Kalman filters work well if you have a reasonable model of the state being measured. If you don’t there are less effective methods, but much simpler:
https://pro.arcgis.com/en/pro-app/l...tatistics/how-time-series-smoothing-works.htm covers the basics conceptually.
For that to work well you might need to convert the angular co-ordinates into a notional position space, but I’d just see what it’s like with the raw values (you can even just do it on each element of the vector in isolation to start with!)

The forward/middle/back bias of the 3 moving average approaches indicates how doing derivatives from the denoised data is tricky for making the derivatives reasonable. I have no experience or maths knowledge there sorry. Try all 3 and see which works.

With the derivative data dealing with a missing data point should be trivial, just follow the curve, if there’s many missed points I suspect the second derivative might need to be damped.

 

[AdrianS]

For the embedded stuff we do, signal filtering is done with simple IIR filters, tuned to best fit the noise we have to deal with.

Probably not as good as a Kalman filter, but easy to implement, and I don't have floating-point hardware, so everything fast has to be integer or fixed-point.

 

[rain shadow]

Maybe a second sensor so you can get stereo positioning?

 

[ajk48n]

hoping I don't run into the gimbal lock problem

Would you be able to convert your Vector3s back and forth between quaternions? That should help with any gimbal lock issues.

Also, are there any assumptions about the Up vector you can make? Like are your robots always horizontal and up is towards the sky? That could also help narrow down any inconsistencies