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
kalman.h
- Committer:
- Rogercl
- Date:
- 2019-07-27
- Revision:
- 5:d87c25f009d1
File content as of revision 5:d87c25f009d1:
#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; }