Position Calculation
The foundation for all further steering decisions is the kite's knowledge about it's own orientation in the real word. Specifically, it needs to know how it's roll-, pitch- and yaw-axis are oriented relative to the real worlds gravity and surface. This knowledge and all calculations on this page are modeled in the Position class. Our axis naming follows the convention for airplanes:
The position information is stored in a 3x3 matrix:
| Roll | Pitch | Yaw | |
|---|---|---|---|
| Gravity | 1 | 0 | 0 |
| Surface | 0 | 1 | 0 |
| Surface | 0 | 0 | 1 |
The table above shows the initial values the kite assumes when it is switched on. Going forward, the kite will constantly update this table based on gyroscope and acceleration measurements (see MPU6050 in Sensors).
The kite primarily measures movements out of which it calculates a deviation which then is applied to the current state (table). This means the respective latest state can be understood as the result of a sequence of updates to the initial state.
It is crutial that during startup (when switched on), the physical kite position is aligned with the kite's assumption about it's physical position. The picture shows how the initial table configuration above maps to the physcal kite position and how the kite must be oriented when switched on.
The position matrix is updated multiple times per second with the latest movements taken from the MPU6050 gyroscope and acceleration measurements. An update consists of the following three steps:
- Apply movements
- Rotate towards gravitation
- Normalization
The first step takes the gyroscope measurements and computes a rotation matrix that expresses the changes that happened since the last update. The rotation matrix is applied to the position matrix. This step in isolation would cause model and realtiy to diverge over time.
The second step uses the acceleration measurements to correct this divergency. The idea is that an arithmetic mean over many acceleration measurements will result in the gravitation vector as everything else is residuals that balance out over time. We do not correct the divergency of the two surface vectors.
A final step of Gram-Schmidt orthogonalization re-enforces that the roll-, pitch- and yaw-axis are orthogonal to each other and have the length of 1.
The gyroscope measurement is represented as vector of three angular velocities (degrees per second), one for roll-, pitch- and yaw-axis each. The vector represents the latest gyroscope reading. The assumption in the upcoming calculation is that these angular velocities were constant for the time interval since the last update.
With this simplification we may
- multiply the velocities with the elapsed time (seconds) since the last update. The result is the rotation of each axis since the last update in degrees.
- multiply with Pi/180 to convert to radians (needed to proceed).
Given the angles we wish each axis to rotate with, we can compute a rotation matrix resembeling these changes and apply it to the position matrix holding the latest state. As the angles can be expected to be very small, we use infinitesimal rotations to simplify the computation of the rotation matrix.
The approach is to assume that the latest measured acceleration equals the gravitation. Based on that assumption, we compute an axis and an angle that represent the rotation that would be required to align model and reality. As we know that the measured acceleration does not equal the gravitation and that we have to take an arithmetic average over many values, the computed angle is weighted with a factor < 0. As the update is performed iteratively multiple times per second, every time the rotation towards gravitation is executed, the model is adjusted a tiny bit. It is adjusted towards the direction of actual acceleration and not the gravitation, but the errors caused will average out as long as the weight is small enough. However, if the weight is chosen too small, the correction effect is not strong enough to oppose the divergency caused by step 1.
Note that we do not want to align any of the kite's axes but the model with gravitation.
The acceleration vector was measured by a sensor which is mounted on the kite. In order to make it comparable to the model's absolute gravity vector (1, 0, 0) we transform it into the model coordinate system by multiplying with the transposed rotation matrix.
The axis is the orthogonal line of the model's absolute gravite vector and the acceleration vector which is assumed to be the measured gravity. We calculate it using the cross-product and normalize to prepare for the infinitesimal rotation matrix.
We first normalize the transformed accelaration vector. The model's absolute gravity vector is already normalized. With both vectors being of length 1, the length of the connecting vector (computed by vector substraction) is approximately the angle in radians. As with the infinitesimal rotation matrix, we waive the applicance of trigonometric functions as the angles are expected to be very small.
Axis and angle are then converted to an infinitesimal rotation matrix (as in step 1) which is applied to the position matrix.
Why is this necessary in the first place? Why is re-orthogonalization necessary? Why exactly do vector lengths change?