Attitude estimate computed on controller instable/unreliable

[skohlbr]

16.04/AMD64/Kinetic master branch Naze32 FC (ordered off ebay, so possibly copy)

I'm observing that the attitude estimate provided by the rosflight FW is pretty unreliable. After resetting via the available service, it works fine for some time, but often starts spinning around some axis after a few minutes (even if the board just sits still on the table). If I feed the raw accelerometer and gyro data to either imu_filter_madgwick or imu_complementary_filter instead, the attitude estimates coming out of those look fine and stay stable. Given this fact, it seems the sensor data should not be the culprit. As my main use for the FC is as a IMU for an autonomous vehicle, I can just use the previously described setup, but it would still be nicer to get already good estimates from the firmware.

[superjax]

That's pretty concerning... Thanks for letting us know!

Do you happen to have a rosbag of this happening? I have seen this in the past, but I thought that I had fixed it (it was too aggressive a gain on FILTER_KP and FILTER_KI.) I bet I still have it pretty close to the margin, because otherwise I feel like the estimator lags... Though it could be something completely different. We fly this on a somewhat regular basis and I haven't seen it in a long while (over a year).

Thanks!

[skohlbr]

I'll be able to get a bag file at some point, but might take a while due to other commitments.

[superjax]

I've just been able to reproduce this behavior. Thanks, definitely my bad - I've dialed down the FILTER_KI parameter to 0.01 and that seems to have fixed it. I'll try running it overnight a few nights on a couple of chips to confirm that fixes it.

[korken89]

Hi, I am seeing the same issue @superjax , also on a NAZE32 (AfroFlight).

By simply hovering, after a while the estimation starts to drift away until it has a bias of about (+/-) 0.2 - 0.3 radians on roll and pitch. Lowering the KI term of the filter does not help, it only makes the issues appear slower. While after landing the estimator converges again to the correct estimation.

I am running this inside our motion capturing arena, so I can provide any logs that you'd like with ground truth.

BR Emil

[superjax]

Thanks, that's really troubling.

Let me make sure I understand - You're observing a bias when flying, but not when stationary?

If you could collect a log, that would be awesome, try to get as much imu data as possible. Our motion capture system is down right now, and probably won't be back up for another month. Thanks!

[korken89]

Hi, I have been testing all day and it seems to be a vibration issue.

I had a cable that was touching the clean frame (making the vibration dampening moot) which caused the issue. This however seems a bit strange, if I understand correctly the internal IMU rate is 1000 Hz which should be far higher than any vibration of the system (plus internal IMU filtering). And by averaging the accelerometer values should still provide a good reference.

Do you have any idea where this might come from?

Also, I can again put the cable back and create logs if you want. Could be good to analyze the problem a bit further :)

[superjax]

That is correct, the imu rate should be 1000 hz.

There is a decent chance that we could miss one or two measurements per second, but probably not in a row, the status/loop_rate_us variable indicates how long the control loop is taking and should let you know if you're missing samples (if it gets larger than 1000 us). The estimator is time-adjusted though and should be able to handle a missing imu sample.

Could it be the low-pass filter messing with the data pre-integration?

[superjax]

Yeah, we should be able to handle a disturbance like that. I really like the notch filtering idea. This might be a good dataset to use to develop it.

[korken89]

Hmm, it could be, and I will make a dataset to test on which exhibits the problem. I was thinking the same about the notch filter, though we could sadly never run it on the NAZE32 based boards but the new port to F4 based boards should solve that issue!

[korken89]

I tried to get it to happen again and was successful, though not at extreme as before, but it shows the effect. I have created a rosbag here https://drive.google.com/open?id=1acsgVGwNRCfrIj9HF0cErhoZFMuiIZxl with:

• IMU at 1000 Hz
• Motion capture at 100 Hz
• All other rosflight topics

I started from motors off and it continues until I have landed with motors off again, plus until the estimator has converged back and eliminated the bias again.

The issue is happening here with approx. 0.12 radians in roll and 0.09 in pitch. The entire experiment I was hovering except once where I returned the roll and pitch stick to resting position to see how much offset there was.

If you have any questions I'm here to help!

[superjax]

Awesome. Thanks so much. I'll hopefully get some time to look at this tonight.

In your testing, did you notice if the bias error always went the same way?

[korken89]

Thanks! I'm quite interested as the estimator you are using is theoretically solid, so perhaps there is some other effect at play.

As far as I tested they went the same way, though not sure if it were the same way in the first trial as I sadly didn't log that.

[superjax]

Ultimately, I believe reason that the filter isn't working very well is that not very many of the IMU measurements are making it into the filter. (check out lines 156-157 of estimator.cpp) The noise on accel is so high that only 6% of the accel measurements are fused.

Check out the IMU data on that log

"filtered" corresponds to the LPF in the firmware. This is with an LPF of 0.5.

However, there is another thing really weird with this log - I'm seeing this really weird flipping between -9.8 and 9.8 on the z axis. It appears to continue throughout the flight where the acceleration vector is randomly changing signs. I haven't been able to see anything like that on my hardware. I bumped the LPF up to 0.999 and got this:

I wouldn't normally expect IMU data on the z-axis to go to zero.

Could you check and see if your IMU data looks right? I'll keep investigating on my end to see if I can observe this in my hardware.

[korken89]

Thank you for the initial analysis! I checked the data and I was running my node to convert everything into NWU frame, /pixyrf/imu is in the NWU frame, while /pixyrf/rosflight/imu is the "raw" rosflight IMU. Sorry for the confusion! Is this perhaps causing the sign flipping you saw?

On the note of high noise, I have a possible suggestion. I took a look at the estimator again and noticed the bounds you are using to reject measurements, and I was thinking that maybe a soft-limit is a better choice for noisy systems?

Like full gains for the 0.9-1.1g but exponential tails rather than full off? Or have the gains be something similar to a Gaussian full gains at 1g and then dropping off. Something that is computationally tractable but also with desired roll-off properties.

Not 100% sure what would be a good weighting strategy though. Have you tested different ones?

[korken89]

Perhaps something like this W = 1 / ( 1 + 10000 * (x - 9.8)^4 )

[superjax]

Haha! oops. Yeah, my bad. I filtered the IMU messages in my rosbag parser and things look a lot better now.

I did a little bit of tuning

ACC_LPF_ALPHA: 0.9
FILTER_KI: 0.01
FILTER_KP: 0.5

and got this plot with the hard cutoff (before)

I then implemented your above change and got this:

I like the y estimates a lot better on this second plot.

Anyway, we now have our motion capture system up and going here, so I might spend some more time dinking around with this. I like the new weighting scheme.

In case you're at all interested, everything I'm using to post-process this is on the fixing_289_unstable_att branch. I'm running the rosbag_parser function built in the test folder

cd test
mkdir build
cd build
cmake ..
make
./rosbag_parser -p /home/superjax/korken_params.yaml -f ~/Downloads/rosflight_bias_in_estimator_with_ground_truth.bag -imu /pixyrf/rosflight/imu

then I plot the results with

cd scripts
python plot_post-process.py

[korken89]

Awesome that you gave it a test! I have been abit swamped at our lab for now, but it seems to indeed be helping. I designed this cutoff gain to be close to what was already implemented, testing to make it not have quite as aggressive falloff would also be interesting.

[mohradza]

Hello!

I think I am having similar problems regarding attitude estimates drifting, and I was hoping to get some help with finding a solution. I am on the master branch of the firmware, and I am using a NAZE32 Rev6 board.

I have been experiencing significant estimated attitude drift during flights. Here are some plots from a flight were I was doing position control using feedback using vicon position estimates in roscopter. You can see that the attitude is continuously drifting, so much that the controller eventually cannot compensate for the error and the vehicle drifts away from its commanded position (-2.6, 0, -1).

What looked most concerning to me was the accelerometer measurements, which are super noisy. I decided to remount the naze board with better vibration dampening foam. While this did reduce the magnitude of the noise, I was still experiencing pretty significant attitude drift over the course of about 10 seconds of the motors spinning up. (see the following figure)

This test was done on the ground without taking off, and the estimates are still drifting significantly. I tried changing the filter parameters mentioned above (ACC_LPF_ALPHA from .5 to .9, and FILTER_KI from .05 to .01), but I did not get results that looked very good to me:

If anyone has any ideas of how I can fix this situation, please let me know! I can provide the rosbags if anyone wants them.

Thanks

[bsutherland333]

I'm not sure if this is still an issue, but this may be useful when we revisit the onboard estimator so I'll leave it open.

[bsutherland333]

Our current hardware doesn't exhibit this problem (possibly due to higher quality sensors), so I'm going to close this issue.