% integrate angular velocity to get angle
dt = seconds(angular_velocities(2) - angular_velocities(1));
for idx = 1 : length(angular_velocities)
    gyro_angle = gyro_angle + ( dt * angular_velocities(idx) );
    gyro_angles(idx) = gyro_angle;
end

% set the first value in the x hat as the accelerometer value
x_hat(1) = accelerometer_angles(1);

% run the ext kalman filter
for idx = 2 : length(angular_velocities)
    accele_angle = accelerometer_angles(idx);
    gyro_angle = gyro_angles(idx);

    % predict
    x_hat(idx) = x_hat(idx - 1);
    P = P + Q;

    % update
    a = P + R_1;
    d = P + R_2;
    e = P / (a*d - P*P);
    k1 = e*d - e*P;
    k2 = e*a - e*P;
    P = (1 - (k1+k2)) * P;

    x_hat(idx) = x_hat(idx) + (k1*(gyro_angle - x_hat(idx))) + (k2*(accele_angle-x_hat(idx)));
end