Algoritmo funcionando com a biblioteca de inatividade utilizando dos dados do acelerômetro e a biblioteca de PeakSearch se utilizando dos dados filtrados pelo filtro Kalman.
Dependencies: mbed MatrixMath Matrix nrf51_rtc BMP180 MPU9250
Diff: kalman.h
- Revision:
- 5:d87c25f009d1
diff -r 215c9d6d1c80 -r d87c25f009d1 kalman.h --- /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