Diff: kalman.h

Revision:

5:d87c25f009d1

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/kalman.h	Sat Jul 27 12:17:25 2019 +0000
@@ -0,0 +1,75 @@
+#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;
+    
+}
\ No newline at end of file