Fixing small issues at ekf_att_pos_estimator

[sverling]

I found some small issues in the state estimator app which I have resolved. It is about three things:

lline 460: the for loop here is a duplicate of the previous for loop
line 777ff: according to comment and having a look at the units states 10-12 are delta angles (in rad). if we want to add it to the angular rates (rad/sec) we have to divide it with dt
line 1059: Here I adapted the the comment such that it is more descriptive and is similar to previous states (delta angle)

[thomasgubler]

Makes sense if those comments are correct. However that would mean that until now the output of the the estimator was way off. Did you plot the angular rates before and after the change?

Also I am confused about why we publish those values twice to the attitude topic. _att.rate_offsets[0 to 3] and _att.rollspeed/ pitchspeed/yawspeed ...

[sverling]

As the biases are very small you probably will not see a big change in the rates. However the bias is now much bigger (but still very small). Before this change we measured a bias of approx. 0.00002 rad (delta angle). If we divide it by dt we have 0.002 rad/sec.

About publishing twice. I think the reason why we publish the offset and the rates separately is that one can not only see the rates but also the offset itself for post analysis.

[LorenzMeier]

@sverling The argument makes perfect sense and your fix is certainly needed. However, do you have some logs showing that it effectively zeros the gyros? I just want to be sure its 100% correct now and not just improved.

[sverling]

We don't have such logs yet. We will do such an experiment as soon as possible.

[Zefz]

Note that the rollspeed, anglespeed and yawspeed are used by the multicopter land_detector module to detect if the vehicle has landed. These parameters in the land_detector might need to be updated for this EKF correction (or else you might get false positives or false negative land detected)

[LorenzMeier]

Thanks!