File content as of revision 6:e9a2bc040ada:

#include "Matrix.h"
#include "MatrixMath.h"
//variaveis inatividade

void kalman(float AcceX, float AcceY, float AcceZ, float GyroX, float GyroY, float GyroZ,  float *Yaw,  float *Pitch, float *Roll, Matrix *P, float dt)
{
    //Matrizes de controle do filtro
    Matrix  R(3,3);
    R << 30  << 0  << 0
      << 0  << 30  << 0
      << 0  << 0  << 30;

    Matrix  Q(3,3);
    Q << 1  << 0  << 0
      << 0  << 1  << 0
     << 0  << 0  << 1;

    //Variaveis auxiliares
    Matrix  C(3,3);
    C << 1  << 0  << 0
      << 0  << 1  << 0
      << 0  << 0  << 1;
    Matrix S(3,3);
    Matrix K(3,3);
    Matrix  P_hat(3,3);
    float Yaw_hat, Pitch_hat, Roll_hat;
    float Acc_Yaw, Acc_Pitch, Acc_Roll;
    float Error_Yaw, Error_Pitch, Error_Roll;
   

    //Calculando predição dos angulos
    Yaw_hat=*Yaw+GyroZ*dt;
    Pitch_hat=*Pitch+GyroX*dt;
    Roll_hat=*Roll+GyroY*dt;

    
    Matrix  A(3,3);
    A << 1  << 0  << GyroZ*dt
             << 0  << 1  << GyroX*dt
             << 0  << 0  << GyroY*dt;

    //Atualizando a matriz P
    P_hat=A*(*P)*MatrixMath::Transpose(A);
    P_hat(1,1)=P_hat(1,1)+Q(1,1);
    P_hat(2,2)=P_hat(2,2)+Q(2,2);
    P_hat(3,3)=P_hat(3,3)+Q(3,3);
    
    //Calculating the angles with accelerometer data
    Acc_Pitch=atan(AcceX/sqrt(AcceY*AcceY+AcceZ*AcceZ))*(180/3.14);
    Acc_Roll=atan(AcceY/sqrt(AcceX*AcceX+AcceZ*AcceZ))*(180/3.14);
    Acc_Yaw=atan(AcceZ/sqrt(AcceY*AcceY+AcceX*AcceX))*(180/3.14);
    //Error between acce data and gyro data
    Error_Yaw=Acc_Yaw-Yaw_hat;
    Error_Pitch=Acc_Pitch-Pitch_hat;
    Error_Roll=Acc_Roll-Roll_hat;
    //Calculating S matrix
    S=C*P_hat*MatrixMath::Transpose(C);
    S(1,1)=S(1,1)+R(1,1);
    S(2,2)=S(2,2)+R(2,2);
    S(3,3)=S(3,3)+R(3,3);

    //Calculating Kalman Coefficient 
    K=P_hat*MatrixMath::Transpose(C);
    K=K*MatrixMath::Inv(S);

    //Calculando Yaw, Pitch and Roll
    *Yaw=Yaw_hat+ K(1,1)*Error_Yaw + K(1,2)*Error_Pitch +K(1,3)*Error_Roll;
    *Pitch=Pitch_hat+ K(2,1)*Error_Yaw + K(2,2)*Error_Pitch +K(2,3)*Error_Roll;
    *Roll=Roll_hat+ K(3,1)*Error_Yaw + K(3,2)*Error_Pitch +K(3,3)*Error_Roll;


    //Calculando P
    *P=-K*C*P_hat;
    
}