Skip to content

Instantly share code, notes, and snippets.

Embed
What would you like to do?
/* 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
Sign up for free to join this conversation on GitHub. Already have an account? Sign in to comment
You can’t perform that action at this time.