Hey,
I've implemented a position controller (with subordinated velocity, attitude and angular velocity controller) within the App-Layer. Everything is working fine, but I have got problems due to inaccurate yaw-estimation (log: stateEstimate.yaw). I'm using the LPS, and the crazyflie is positioned in x-axis direction when turning on, respectively with yaw angle = 0°. But after just a few seconds of flying, the estimation of yaw-angle becomes very inaccurate. For example, when I'm doing a 90°-rotation to the left (target: yaw = 90°) and after that a 90°-rotation to the right (target: yaw = 0°), the crazyflie's yaw-attitude differs from the initial attitude by approx 20°. How can I fix this? Furthermore, the yaw-angle drifts when the crazyflie is on the ground. Is there a possibility to reduce that drift? Has somebody tried to use the magnetometer instead?
The position controller needs the sine and cosine of yaw-angle in ordner to calculate the pitch- and roll-angle dependent on yaw-angle (similar as it is implemented in position_controller_pid.c https://github.com/bitcraze/crazyflie-f ... pid.c#L173). I'm wondering why the original position controller is working well, because it is using the same yaw-estimation.
I would appreciate any kind of help!
Joyner
LPS: inaccurate estimation of yaw-angle/yaw-drift
-
kristoffer
- Bitcraze
- Posts: 630
- Joined: Tue Jun 30, 2015 7:47 am
Re: LPS: inaccurate estimation of yaw-angle/yaw-drift
Hi!
Since the LPS system only measures distances to anchors (TOF), there is no direct input of data that provides yaw information. The yaw estimate is mainly based on gyro information but also on motion in space. If the Crazyflie is moving in some direction and does not end up at the expected position, the kalman filter will adjust the yaw for a better estimation, but this is a slow process. Never the less, you could say that the more the Crazyflie moves around, the better yaw estimate.
In you case, when you yaw 90 degrees and then back i would not assume that the error would be 20 degrees. Not sure why you see this, could it be due to unbalanced propellers?
When the Crazyflie is on the ground you see an expected behaviour. The kalman filter assumes it is oriented in yaw=0 orientation and slowly "leaks" to this orientation. You can change how fast it goes back here https://github.com/bitcraze/crazyflie-f ... core.c#L76
If you want it to change the direction the CF is facing (other than positive X) you can set the kalman.initialYaw parameter (https://github.com/bitcraze/crazyflie-f ... #L155-L160)
Since the LPS system only measures distances to anchors (TOF), there is no direct input of data that provides yaw information. The yaw estimate is mainly based on gyro information but also on motion in space. If the Crazyflie is moving in some direction and does not end up at the expected position, the kalman filter will adjust the yaw for a better estimation, but this is a slow process. Never the less, you could say that the more the Crazyflie moves around, the better yaw estimate.
In you case, when you yaw 90 degrees and then back i would not assume that the error would be 20 degrees. Not sure why you see this, could it be due to unbalanced propellers?
When the Crazyflie is on the ground you see an expected behaviour. The kalman filter assumes it is oriented in yaw=0 orientation and slowly "leaks" to this orientation. You can change how fast it goes back here https://github.com/bitcraze/crazyflie-f ... core.c#L76
If you want it to change the direction the CF is facing (other than positive X) you can set the kalman.initialYaw parameter (https://github.com/bitcraze/crazyflie-f ... #L155-L160)
Re: LPS: inaccurate estimation of yaw-angle/yaw-drift
Thanks for your help!
I set the symbol "ROLLPITCH_ZERO_REVERSION" to 0. Everything is working perfectly now. Maybe the inaccurate Estimation was due to the fact, that I commented out the function call "controller(&control, &setpoint, &sensorData, &state, tick);" in stabilizer.c (https://github.com/bitcraze/crazyflie-f ... zer.c#L282. Maybe the Kalman-Filter was missing some Information, or it was thinking that the crazyflie is on the ground ... Anyways, the controller works perfectly now. Thanks a lot!
I set the symbol "ROLLPITCH_ZERO_REVERSION" to 0. Everything is working perfectly now. Maybe the inaccurate Estimation was due to the fact, that I commented out the function call "controller(&control, &setpoint, &sensorData, &state, tick);" in stabilizer.c (https://github.com/bitcraze/crazyflie-f ... zer.c#L282. Maybe the Kalman-Filter was missing some Information, or it was thinking that the crazyflie is on the ground ... Anyways, the controller works perfectly now. Thanks a lot!