koordinatsystemkrøll.c

/* Read acc and gyro */
body_linear_acc /* contains: u_dot, v_dot, w_dot */
body_rotation_acc /* contains: p_dot, q_dot, r_dot */

/* Calculate angle TODO: compensate for turning */
local_angle = find_angles_atan2(body_linear_acc)

/* Read GPS position and speed */
gps_speed /*contains x, y, z */
gps_position /* contains x_dot, y_dot, z_dot */
gps_bearing

/* Transform to Local Tangent Plane */
gps_speed = ecef_to_ltp(gps_speed)
gps_position = ecef_to_ltp(gps_position)

/* Average rotation_acc */
body_avg_rotation_acc = rk4_slope(body_rotation_acc)
local_avg_rotation_acc = angle_body_to_local(body_avg_rotation_acc)

/* Estimate angle */
local_angle = kalman(local_angle, body_rotation_acc)

/* Angles and angular acceleration in euler angles */
euler_angle = local_to_euler(local_angle, gps_bearing)
euler_rotation_acc = local_to_euler(local_avg_rotation_acc)

/* Transform linear acceleration to local system */
local_linear_acc = linear_body_to_local(body_linear_acc, euler_angle)

/* Integrate to find position */
velocity = kalman(gps_speed, local_linear_acc)
position = kalman(gps_position, velocity)


OUT:
position
velocity
euler_angle
euler_rotation_acc