main/control/Position.cpp
// Align model gravitation with measured gravitation.
// Measured gravitation results from averaging accel values over many iterations.
Align model gravitation with measured gravitation.
Measured gravitation results from averaging accel values over many iterations.
Align model gravitation with measured gravitation . Measured gravitation results from averaging accel values over many iterations .
void Position::rotate_towards_g(array<float, 3> accel, PositionUpdate& out) {
    array<float, 3> accel_in_world_coordinates {Matrix3::multiply(matrix, accel)};
    out.g_correction_axis = Vector3::cross_product(accel_in_world_coordinates, init_gravity);
    Vector3::normalize(out.g_correction_axis, 0.000001);
    Vector3::normalize(accel_in_world_coordinates);
    array<float, 3> rotation_vector { Vector3::subtract(accel_in_world_coordinates, init_gravity) };
    out.g_correction_angle = Vector3::get_norm(rotation_vector) * accel_gravity_weight;
    array<float, 9> difference {
        1,
        -out.g_correction_axis[2] * out.g_correction_angle,
        out.g_correction_axis[1] * out.g_correction_angle,
        out.g_correction_axis[2] * out.g_correction_angle,
        1,
        -out.g_correction_axis[0] * out.g_correction_angle,
        -out.g_correction_axis[1] * out.g_correction_angle,
        out.g_correction_axis[0] * out.g_correction_angle,
        1
    };
    matrix = Matrix3::multiply(difference, matrix);
}
void Position :: rotate_towards_g ( array < float , 3 > accel , PositionUpdate & out ) { array < float , 3 > accel_in_world_coordinates { Matrix3 :: multiply ( matrix , accel ) } ; out . g_correction_axis = Vector3 :: cross_product ( accel_in_world_coordinates , init_gravity ) ; Vector3 :: normalize ( out . g_correction_axis , 0.000001 ) ; Vector3 :: normalize ( accel_in_world_coordinates ) ; array < float , 3 > rotation_vector { Vector3 :: subtract ( accel_in_world_coordinates , init_gravity ) } ; out . g_correction_angle = Vector3 :: get_norm ( rotation_vector ) * accel_gravity_weight ; array < float , 9 > difference { 1 , - out . g_correction_axis [ 2 ] * out . g_correction_angle , out . g_correction_axis [ 1 ] * out . g_correction_angle , out . g_correction_axis [ 2 ] * out . g_correction_angle , 1 , - out . g_correction_axis [ 0 ] * out . g_correction_angle , - out . g_correction_axis [ 1 ] * out . g_correction_angle , out . g_correction_axis [ 0 ] * out . g_correction_angle , 1 } ; matrix = Matrix3 :: multiply ( difference , matrix ) ; }
Align model gravitation with measured gravitation.
Align model gravitation with measured gravitation .
// acceleration was measured by a sensor mounted on the kite, hence: // Compute rotation axis // Compute rotation angle // Compute infinitesimal rotation matrix from given axis and angle // Apply