diff --git a/docs/functional-areas/sensor-to-control/state_estimators.md b/docs/functional-areas/sensor-to-control/state_estimators.md index 7cfdd6d0df..57391002bb 100644 --- a/docs/functional-areas/sensor-to-control/state_estimators.md +++ b/docs/functional-areas/sensor-to-control/state_estimators.md @@ -5,7 +5,7 @@ page_id: state_estimators -A state estimator turns sensor signals into an estimate of the state that the crazyflie is in. This is an essential part of crazyflie's stabilizing system, as explained in the [overview page](/docs/functional-areas/sensor-to-control/index.md). State estimation is really important in quadrotors (and robotics in general). The Crazyflie needs to first of all know in which angles it is at (roll, pitch, yaw). If it would be flying at a few degrees slanted in roll, the crazyflie would accelerate into that direction. Therefore the controller need to know an good estimate of current angles’ state and compensate for it. For a step higher in autonomy, a good position estimate becomes important too, since you would like it to move reliably from A to B. +A state estimator turns sensor signals into an estimate of the state that the crazyflie is in. This is an essential part of crazyflie's stabilizing system, as explained in the [overview page](/docs/functional-areas/sensor-to-control/index.md). State estimation is really important in quadrotors (and robotics in general). The Crazyflie needs to know first of all in which angles it is at (roll, pitch, yaw). If it would be flying at a few degrees slanted in roll, the crazyflie would accelerate into that direction. Therefore the controller needs to know good estimate of current angles’ state and compensate for it. For a step higher in autonomy, a good position estimate becomes important too, since you would like it to move reliably from A to B. * [Complementary filter](#complementary-filter) * [Extended Kalman filter](#extended-kalman-filter) @@ -16,25 +16,25 @@ A state estimator turns sensor signals into an estimate of the state that the cr ![complementary filter](/docs/images/complementary_filter.png){:width="500"} -The complementary filter is consider a very lightweight and efficient filter which in general only uses the IMU input of the gyroscope (angle rate) and the accelerator. The estimator has been extended to also include input of the ToF distance measurement of the [Zranger deck](https://store.bitcraze.io/collections/decks/products/z-ranger-deck-v2). The estimated output is the Crazyflie’s attitude (roll, pitch, yaw) and its altitude (in the z direction). These values can be used by the controller and are meant to be used for manual control. +The complementary filter is very lightweight and efficient filter which in general only uses the IMU output of the gyroscope (angle rate) and the accelerometer. The estimator has been extended to also include output of the ToF distance measurement of the [Zranger deck](https://store.bitcraze.io/collections/decks/products/z-ranger-deck-v2). The estimated output is the Crazyflie’s attitude (roll, pitch, yaw) and its altitude (in the z direction). These values can be used by the controller and are meant to be used for manual control. -To checkout the implementation details, please checkout the firmware in `estimator_complementary.c` and `sensfusion6.c`. The complementary filter is set as the default state estimator on the Crazyflie firmware, unless a deck is mounted that requires the Extended Kalman filter. +To checkout the implementation details, please look the firmware in `estimator_complementary.c` and `sensfusion6.c`. The complementary filter is set as the default state estimator on the Crazyflie firmware, unless a deck is mounted which requires the Extended Kalman filter. ## Extended Kalman filter ![extended kalman filter](/docs/images/extended_kalman_filter.png){:width="500"} -The (extended) Kalman filter (EKF) is an step up in complexity compared to the complementary filter, as it accepts more sensor inputs of both internal and external sensors. It is an recursive filter that estimates the current state of the Crazyflie based on incoming measurements (in combination with a predicted standard deviation of the noise), the measurement model and the model of the system itself. +The (extended) Kalman filter (EKF) is step up in complexity compared to the complementary filter, as it accepts more sensor outputs of both internal and external sensors. It is a recursive filter that estimates the current state of the Crazyflie based on incoming measurements (in combination with a predicted standard deviation of the noise), the measurement model and the model of the system itself. We will not go into detail on this but we encourage people to learn more about EKFs by reading up some material like [this](https://idsc.ethz.ch/education/lectures/recursive-estimation.html). -Because of more state estimation possibilities, we prefer the EKF for certain decks that can provide information for **full pose estimation** (position/velocity + attitude). These are the: [Flowdeck v2](https://store.bitcraze.io/collections/decks/products/flow-deck-v2), [Loco positioning deck](https://store.bitcraze.io/collections/positioning/products/loco-positioning-deck), [Lighthouse deck](https://store.bitcraze.io/products/lighthouse-positioning-deck), Mocap deck [passive](https://store.bitcraze.io/products/motion-capture-marker-deck) / [active](https://store.bitcraze.io/products/active-marker-deck). The estimator preferences of these decks are set in their [deck api](/docs/userguides/deck.md). +Because of more state estimation possibilities, we prefer the EKF for certain decks that can provide information for **full pose estimation** (position/velocity + attitude). These are: [Flowdeck v2](https://store.bitcraze.io/collections/decks/products/flow-deck-v2), [Loco positioning deck](https://store.bitcraze.io/collections/positioning/products/loco-positioning-deck), [Lighthouse deck](https://store.bitcraze.io/products/lighthouse-positioning-deck), Mocap deck [passive](https://store.bitcraze.io/products/motion-capture-marker-deck) / [active](https://store.bitcraze.io/products/active-marker-deck). The estimator preferences of these decks are set in their [deck api](/docs/userguides/deck.md). -Here we will explain a couple of important elements that are essential to the implementation, however we do encourage people to also look into the code `estimator_kalman.c` and `kalman_core.c`. Also read the papers of [1] and [2] for implementation details. +Here we will explain a couple of important elements that are essential to the implementation, however we do encourage people also look into the code `estimator_kalman.c` and `kalman_core.c`. Also read the papers [1] and [2] for implementation details. ### Kalman supervisor -The Kalman filter has an supervisor that resets the state estimation if the values get out of bounds. This can be found here in `kalman_supervisor.c`. +The Kalman filter has a supervisor that resets the state estimation if the values get out of bounds. This can be found here in `kalman_supervisor.c`. ### Measurement Models @@ -55,7 +55,7 @@ This illustration explains how the height from the VL53L1x sensor and flow from **NOTE** *This is still in the experimental phase, so it would need to be enabled in [kbuild](/docs/development/kbuild.md) in 'Controllers and Estimators' and Enable error-state UKF estimator'* -The error-state unscented Kalman Filter (error-state UKF) is another navigational algorithm for the Crazyflie whose objective is improving navigation quality when relying on the Loco-Positioning system. It combines a strapdown navigation algorithm [4] performing a time integration of the accelerometer and gyrosope measurements with an unscented Kalman-Filter estimating the error-state between the strapdown navigation solution and the true position by means of the deck measurements. Just like the EKF described above the UKF is a recursive filter. However, compared to an EKF approach, the UKF does not rely on linearizing the nonlinear measurement equations but instead employs deterministically chosen sigma-points to approximate the mean and the covariance of the error-state. This enhances the approximation accuracy, which turns out to be particularly advantageous when using the Loco-Positioning-System for absolute positioning. The algorithm is described in more detail in reference [5]. Further details on deriving error-state predictions models as well as a more detailed derivation of the UKF approach can be found in [6] and [7]. +The error-state unscented Kalman Filter (error-state UKF) is another navigational algorithm for the Crazyflie whose objective is improving navigation quality when relying on the Loco-Positioning system. It combines a strapdown navigation algorithm [4] performing a time integration of the accelerometer and gyrosope measurements with an unscented Kalman-Filter estimating the error-state between the strapdown navigation solution and the true position by means of the deck measurements. Just like the EKF described above the UKF is a recursive filter. However, compared to an EKF approach, the UKF does not rely on linearizing the nonlinear measurement equations but instead employs deterministically chosen sigma-points to approximate the mean and the covariance of the error-state. This enhances the approximation accuracy, which turns out to be particularly advantageous when using the Loco-Positioning-System for absolute positioning. The algorithm is described in more details in reference [5]. Further details on deriving error-state predictions models as well as more detailed derivation of the UKF approach can be found in [6] and [7]. In its current implementation, the algorithm is able to fuse the following measurements: