File content as of revision 3:02877f5ca29e:

#include "mbed.h"
#include "kalman.c"
#include "x_nucleo_iks01a1.h"
#include <math.h>

#define PI             3.1415926535897932384626433832795
#define Rad2Dree       57.295779513082320876798154814105

/* Instantiate the expansion board */
static X_NUCLEO_IKS01A1 *mems_expansion_board = X_NUCLEO_IKS01A1::Instance(D14, D15);

/* Retrieve the composing elements of the expansion board */
static GyroSensor *gyroscope = mems_expansion_board->GetGyroscope();
static MotionSensor *accelerometer = mems_expansion_board->GetAccelerometer();
static MagneticSensor *magnetometer = mems_expansion_board->magnetometer;

Serial pc(USBTX, USBRX);

Timer GlobalTime;
Timer ProgramTimer;

float R;
double angle[3];
unsigned long timer;
long loopStartTime;
int32_t axesAcc[3];
int32_t axesGyro[3];
int32_t axesMag[3];

kalman filter_roll;
kalman filter_pitch;
kalman filter_yaw; 
    
int main() {
    pc.baud(115200);
  
//    GlobalTime.start();

    // Parameters ( R_angle, Q_gyro, Q_angle ) 
    kalman_init(&filter_pitch, R_matrix, Q_Gyro_matrix, Q_Accel_matrix); 
    kalman_init(&filter_roll, R_matrix, Q_Gyro_matrix, Q_Accel_matrix); 
    
    kalman_init(&filter_yaw, R_matrix, Q_Gyro_matrix, Q_Accel_matrix);
    
    
    ProgramTimer.start();
    loopStartTime = ProgramTimer.read_us();
    timer = loopStartTime;
    
    while(1) {
//    wait_ms(10);
        //Aquire new values for the Gyro and Acc
          accelerometer->Get_X_Axes(axesAcc);
          gyroscope->Get_G_Axes(axesGyro);
          magnetometer->Get_M_Axes(axesMag);
        
        //Calculate the resulting vector R from the 3 acc axes
        R = sqrt(std::pow((float)axesAcc[0] , 2) + std::pow((float)axesAcc[1] , 2) + std::pow((float)axesAcc[2] , 2));
   
        kalman_predict(&filter_roll, axesGyro[0],  (ProgramTimer.read_us() - timer)); 
        kalman_update(&filter_roll, acos(axesAcc[0]/R));
        
        kalman_predict(&filter_pitch, axesGyro[1],  (ProgramTimer.read_us() - timer)); 
        kalman_update(&filter_pitch, acos(axesAcc[1]/R));
        
        kalman_predict(&filter_yaw, axesGyro[2],  (ProgramTimer.read_us() - timer)); 
        kalman_update(&filter_yaw, acos(axesMag[2]/R));
        
        
        angle[0] = kalman_get_angle(&filter_roll);
        angle[1] = kalman_get_angle(&filter_pitch);
        angle[2] = atan2((double)axesGyro[0],(double)axesGyro[1])*Rad2Dree;
//        angle[2] = kalman_get_angle(&filter_yaw);
        atan2((double)axesAcc[0],(double)axesAcc[1])*Rad2Dree; 
                
        timer = ProgramTimer.read_us();
        
        pc.printf("Roll Angle X: %.6f   Pitch Angle Y: %.6f Yaw Angle Z: %.6f \n\r", Rad2Dree * angle[0], Rad2Dree * angle[1], Rad2Dree * angle[2]);
        
    }
}