Skip to content
This repository has been archived by the owner on May 1, 2024. It is now read-only.

Rotate yaw in position control mode,the horizontal velocity is wrong with gps measured velocity. #980

Closed
garlinplus opened this issue Feb 18, 2021 · 7 comments

Comments

@garlinplus
Copy link

garlinplus commented Feb 18, 2021

No description provided.

@garlinplus
Copy link
Author

@CarlOlsson
Copy link
Contributor

That's a very interesting logfile, the IMU biases diverge during the rotation. Do you think you could collect a replay logfile during the same maneuver?

@garlinplus
Copy link
Author

garlinplus commented Feb 18, 2021

the IMU biases diverge during the rotation
image
The gyro and acc bias seems to be large than formal flying log as followed:
image

@garlinplus
Copy link
Author

That's a very interesting logfile, the IMU biases diverge during the rotation. Do you think you could collect a replay logfile during the same maneuver?

The imu bias is larger than formal flying,but Even if as that,the gps and mag measure can not correct those with ekf measure update process?

@abishekvupputur
Copy link

abishekvupputur commented Apr 1, 2021

I notice that your yaw angular rate is about 100 deg/s. I think it is quite possible that you are measuring centrifugal acceleration in the body reference frame. Due to lack of observability of centrifugal force, in body frame it is interpreted like a "added bias (function of spin) during large yaw rates". So it makes sense that the bias estimates in [x,y] is diverging to accommodate for this error caused as a consequence of spin. If this is the case, I think the EKF is working perfectly to accommodate this. There 2 possible ways to get rid of this problem.

  1. Centre your IMU closer to the center of gravity of the drone. The closer you are, lower is your "acceleration bias divergence".
  2. Write a small calibration routine, manually calculate the offset of imu from the centre of gravity and calculate your acceleration caused as a result of spin before passing the values through your EKF. This compensation for the added acceleration can be done by subtracting a_c from the measured acceleration from IMU:

a_c = ω x ω x r + ω_dot x r.
a_c ~ ω x ω x r
where,
a_c - calculated acceleration in [x,y,z]
ω - body angular velocity in [x,y,z] (gyro bias corrected values)
r - imu offset from cog in [x,y,z]

Note: I ignored the noisy angular acceleration part here ω_dot, the gyroscope values are noisy enough and the derivative can make it worse.

Does this fix your error?

@abishekvupputur
Copy link

abishekvupputur commented Apr 1, 2021

I think this is also a potential duplicate of: #621

@priseborough
Copy link
Collaborator

This looks like the issue that was fixed by #1026

Please re-open if not fixed for you.

Sign up for free to subscribe to this conversation on GitHub. Already have an account? Sign in.
Labels
None yet
Projects
None yet
Development

No branches or pull requests

4 participants