I have been reviewing the LPE code, and had a question about when faults are used and when they aren’t. It seems to me that when a sensor gets a fault, that sensor data should not be fused. However, in the LPE itself, this is only sometimes the case.
In the sensor files, in the BlockLocalPositionEstimator::*sensor*Correct() function, at the bottom where it does the Kalman filter equations, some sensors check for a fault and some fuse always. Why is this?
The following sensors always fuse, regardless if there is a fault or not:
Barometer
GPS
Land Detector
Lidar
MoCap
And the following only fuse if there was not a fault:
Optical Flow
Sonar
Vision
Does anyone have an explanation for this discrepancy? And any explanation for why you would ever want to fuse a sensor if it is throwing a fault?
This fault is based on the residual and the chi squared distribution, not a low level sensor error. Sometimes if the filter gets in an odd state you still want to trust certain sensors.
Yeah I understand that it isn’t necessarily a sensor fault. But the sensor uncertainty and filter uncertainty both go into the math. This “fault” is really an outlier rejection method. If the filter is in a “bad state” as you say, the uncertainty should be high relative to the sensor uncertainty and the measurement should be trusted and therefore not throw a fault. This should all fall out of the math of Kalman filtering, not be explicitly overridden.
If this fault state is triggered, it means that the sensor value is far away from the estimator value, accounting for the uncertainties to determine how far “too far” is. So why would you want to trust this sensor value in this case?
I don’t disagree with you theoretically but currently after testing this is what most users find robust. Originally I did have them all enabled. Feel free to turn them back on as you wish.
OK, I was just curious if there was some theory behind this decision that I wasn’t understanding before I started playing around with it. I’ll try it out and see what happens.
