Have a look at this file in one of my repos https://github.com/BBUK/Bell-Boy/blob/m ... b_dcmimu.c
. Specifically the calculate function. This implements an EKF from https://github.com/hhyyti/dcm-imu
with what I found to be really stunning results for my use case. It is not for the MPU6050 (it uses another IMU) but the calculate function can accept inputs from anywhere, you just need to supply it with gyro measurements (in degrees/sec) and accelerometer measurements (in g) and a time interval (in seconds). Outputs are roll, pitch and yaw in degrees.
The file is WIP and is for a project that I am only going to properly release in a couple of months so the file may change (but the calculate function won't - I absolutely don't understand what magic it performs!).