Using a basestation facing upwards, and the crazyflie upside down.

[knmcguire]

Discussed in https://github.com/orgs/bitcraze/discussions/1518

^{Originally posted by cafeciaojoe August 16, 2024}
Hello all!

I have conducted a basic test to see how stable the lighthouse system is when the are three basestations, with one of the base stations facing upwards. The test involves roughly estimating the variance of the "stablilizer.roll" and the "stateEstimateZ.z" variables whilst the crazyflie is mounted on a tripod facing up (about 0deg roll), sideways (about -90deg roll), and upside down ( about -135deg roll).

The system was calibrated using the wizard, in the latest version of the client and using the latest firmware on a cf 2.1. The third basestation was not visible for the first three steps of the wizard (ie origin, origin + 1m and n number of XY plane measurements)

I encountered some major variance during this experiment when the crazyflie was using the basestation facing up.

There was some insane "oscillating variance" (9deg roll and 200mm height), that was fixed with a restart of the crazyflie, but then some significant and slightly more random variance after the restart (not in the roll, but 50mm in the .z)

if you scrub thru this video you can see how I collected the data. Let me know how I can improve this experiment and provide better data for our diagnosis

https://youtu.be/GtufDYDOZto

Thanks all !
Joe

[gemenerik]

Geometry estimation

Overall, I am very impressed with the geometry estimation process. In tests, I have successfully used a mix of upward- and downward-facing base stations, including setups with highly concave volumes (e.g., upward-facing base stations positioned in the middle of the total covered volume). The system handled these configurations well, showing flexibility in adapting to complex arrangements.

There appears to be no inherent issue with the geometry estimation itself, nor are there strict assumptions about the orientation of base stations. While we generally recommend holding the Lighthouse deck horizontally during data gathering, results are not significantly impacted by tilting the drone. This can be helpful for stitching the upward-facing base station data into the downward-facing ones.

Additionally, I have a gut feeling—shared with @cafeciaojoe—that incorporating upward-facing base stations in the initial ground estimates could be beneficial. Even when these base stations are oriented significantly upwards, the sweeps can still cover ground-level points.

State estimation

The primary issue seems to arise in the state estimation. When using the Lighthouse system, we enable the EKF (Extended Kalman Filter) by default. In the prediction step, the EKF uses accelerometer data to predict the next velocity and position. Since the accelerometer also measures gravity, we account for this in the EKF. However, because the cancellation of gravity relies on the imperfect orientation estimate, gravity does not fully cancel out the measured acceleration. Because of this, the estimated velocity and thus position drifts, growing over time until the Kalman filter resets after reaching out-of-bounds values. This is especially noticeable wen the Crazyflie is static.

This drift increases with rotation away from the initial, level orientation. For instance, if the drone is placed statically at an angle immediately after initialization (which should always be done level), the drift accelerates, leading to a faster reset. Each Kalman reset reinitializes to a level orientation. However, if the drone in reality remains tilted during this reset, the EKF diverges even faster, leading to an oscillation of drift and reset cycles.

Achieving good Lighthouse coverage can counter this drift regardless of the orientation of the base stations and drone. However, at steeper angles, the drift effect becomes stronger, and coverage by multiple Lighthouse base station may be required to stabilize the estimate.

This effect is compounded by the fact that, when the Crazyflie is static (not flying), the state estimation considers accelerations along all three axes—X, Y, and Z. As a result, when the Crazyflie is placed at an angle while stationary, drift occurs not only in the Z axis but also in the X and Y axes, further amplifying the drift effect.

In contrast, when the Crazyflie is flying, the state estimation only considers accelerations along the body Z axis, based on the assumption that the propellers can only produce acceleration in this direction. This reduction to a single axis minimizes drift during flight but is unavailable when stationary, leading to more pronounced drift when at high angles in a non-flying state.

Potential solutions and considerations

Improving Lighthouse coverage is one straightforward way to counteract this drift effect. However, it may also help to understand why the drift increases with tilt. One possible reason is that, since the Kalman reset initializes at a level orientation, the orientation estimate is more accurate when level. As the Crazyflie tilts, small imperfections in sensor accuracy and orientation estimation accumulate, making gravity cancellation less precise. Introducing some form of calibration to reduce drift, particularly at higher tilt angles, could be highly beneficial.

The code currently includes all acceleration components (X, Y, and Z) in the non-flying state, noting that this allows for hand-guided movements of the Crazyflie. While this is a valid use case, the current implementation leads to significant drift issues, suggesting that a static assumption could actually perform better in some cases. However, it’s worth considering the specific use case.

Finally, using a fully static model—ignoring all accelerations—does improve position stability but performs poorly in filling gaps in Lighthouse coverage. Additionally, it impacts orientation estimation, causing significant errors after even small, dynamic rotations (up to 10-20 degrees in a quick test) and a slow convergence back to the correct orientation.