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@5:d87c25f009d1, 2019-07-27 (annotated)
- Committer:
- Rogercl
- Date:
- Sat Jul 27 12:17:25 2019 +0000
- Revision:
- 5:d87c25f009d1
PeakSearch working with Kalman. The best Treshold is 30
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
Rogercl | 5:d87c25f009d1 | 1 | #include "Matrix.h" |
Rogercl | 5:d87c25f009d1 | 2 | #include "MatrixMath.h" |
Rogercl | 5:d87c25f009d1 | 3 | //variaveis inatividade |
Rogercl | 5:d87c25f009d1 | 4 | |
Rogercl | 5:d87c25f009d1 | 5 | void kalman(float AcceX, float AcceY, float AcceZ, float GyroX, float GyroY, float GyroZ, float *Yaw, float *Pitch, float *Roll, Matrix *P, float dt) |
Rogercl | 5:d87c25f009d1 | 6 | { |
Rogercl | 5:d87c25f009d1 | 7 | //Matrizes de controle do filtro |
Rogercl | 5:d87c25f009d1 | 8 | Matrix R(3,3); |
Rogercl | 5:d87c25f009d1 | 9 | R << 30 << 0 << 0 |
Rogercl | 5:d87c25f009d1 | 10 | << 0 << 30 << 0 |
Rogercl | 5:d87c25f009d1 | 11 | << 0 << 0 << 30; |
Rogercl | 5:d87c25f009d1 | 12 | |
Rogercl | 5:d87c25f009d1 | 13 | Matrix Q(3,3); |
Rogercl | 5:d87c25f009d1 | 14 | Q << 1 << 0 << 0 |
Rogercl | 5:d87c25f009d1 | 15 | << 0 << 1 << 0 |
Rogercl | 5:d87c25f009d1 | 16 | << 0 << 0 << 1; |
Rogercl | 5:d87c25f009d1 | 17 | |
Rogercl | 5:d87c25f009d1 | 18 | //Variaveis auxiliares |
Rogercl | 5:d87c25f009d1 | 19 | Matrix C(3,3); |
Rogercl | 5:d87c25f009d1 | 20 | C << 1 << 0 << 0 |
Rogercl | 5:d87c25f009d1 | 21 | << 0 << 1 << 0 |
Rogercl | 5:d87c25f009d1 | 22 | << 0 << 0 << 1; |
Rogercl | 5:d87c25f009d1 | 23 | Matrix S(3,3); |
Rogercl | 5:d87c25f009d1 | 24 | Matrix K(3,3); |
Rogercl | 5:d87c25f009d1 | 25 | Matrix P_hat(3,3); |
Rogercl | 5:d87c25f009d1 | 26 | float Yaw_hat, Pitch_hat, Roll_hat; |
Rogercl | 5:d87c25f009d1 | 27 | float Acc_Yaw, Acc_Pitch, Acc_Roll; |
Rogercl | 5:d87c25f009d1 | 28 | float Error_Yaw, Error_Pitch, Error_Roll; |
Rogercl | 5:d87c25f009d1 | 29 | |
Rogercl | 5:d87c25f009d1 | 30 | |
Rogercl | 5:d87c25f009d1 | 31 | //Calculando predição dos angulos |
Rogercl | 5:d87c25f009d1 | 32 | Yaw_hat=*Yaw+GyroZ*dt; |
Rogercl | 5:d87c25f009d1 | 33 | Pitch_hat=*Pitch+GyroX*dt; |
Rogercl | 5:d87c25f009d1 | 34 | Roll_hat=*Roll+GyroY*dt; |
Rogercl | 5:d87c25f009d1 | 35 | |
Rogercl | 5:d87c25f009d1 | 36 | |
Rogercl | 5:d87c25f009d1 | 37 | Matrix A(3,3); |
Rogercl | 5:d87c25f009d1 | 38 | A << 1 << 0 << GyroZ*dt |
Rogercl | 5:d87c25f009d1 | 39 | << 0 << 1 << GyroX*dt |
Rogercl | 5:d87c25f009d1 | 40 | << 0 << 0 << GyroY*dt; |
Rogercl | 5:d87c25f009d1 | 41 | |
Rogercl | 5:d87c25f009d1 | 42 | //Atualizando a matriz P |
Rogercl | 5:d87c25f009d1 | 43 | P_hat=A*(*P)*MatrixMath::Transpose(A); |
Rogercl | 5:d87c25f009d1 | 44 | P_hat(1,1)=P_hat(1,1)+Q(1,1); |
Rogercl | 5:d87c25f009d1 | 45 | P_hat(2,2)=P_hat(2,2)+Q(2,2); |
Rogercl | 5:d87c25f009d1 | 46 | P_hat(3,3)=P_hat(3,3)+Q(3,3); |
Rogercl | 5:d87c25f009d1 | 47 | |
Rogercl | 5:d87c25f009d1 | 48 | //Calculating the angles with accelerometer data |
Rogercl | 5:d87c25f009d1 | 49 | Acc_Pitch=atan(AcceX/sqrt(AcceY*AcceY+AcceZ*AcceZ))*(180/3.14); |
Rogercl | 5:d87c25f009d1 | 50 | Acc_Roll=atan(AcceY/sqrt(AcceX*AcceX+AcceZ*AcceZ))*(180/3.14); |
Rogercl | 5:d87c25f009d1 | 51 | Acc_Yaw=atan(AcceZ/sqrt(AcceY*AcceY+AcceX*AcceX))*(180/3.14); |
Rogercl | 5:d87c25f009d1 | 52 | //Error between acce data and gyro data |
Rogercl | 5:d87c25f009d1 | 53 | Error_Yaw=Acc_Yaw-Yaw_hat; |
Rogercl | 5:d87c25f009d1 | 54 | Error_Pitch=Acc_Pitch-Pitch_hat; |
Rogercl | 5:d87c25f009d1 | 55 | Error_Roll=Acc_Roll-Roll_hat; |
Rogercl | 5:d87c25f009d1 | 56 | //Calculating S matrix |
Rogercl | 5:d87c25f009d1 | 57 | S=C*P_hat*MatrixMath::Transpose(C); |
Rogercl | 5:d87c25f009d1 | 58 | S(1,1)=S(1,1)+R(1,1); |
Rogercl | 5:d87c25f009d1 | 59 | S(2,2)=S(2,2)+R(2,2); |
Rogercl | 5:d87c25f009d1 | 60 | S(3,3)=S(3,3)+R(3,3); |
Rogercl | 5:d87c25f009d1 | 61 | |
Rogercl | 5:d87c25f009d1 | 62 | //Calculating Kalman Coefficient |
Rogercl | 5:d87c25f009d1 | 63 | K=P_hat*MatrixMath::Transpose(C); |
Rogercl | 5:d87c25f009d1 | 64 | K=K*MatrixMath::Inv(S); |
Rogercl | 5:d87c25f009d1 | 65 | |
Rogercl | 5:d87c25f009d1 | 66 | //Calculando Yaw, Pitch and Roll |
Rogercl | 5:d87c25f009d1 | 67 | *Yaw=Yaw_hat+ K(1,1)*Error_Yaw + K(1,2)*Error_Pitch +K(1,3)*Error_Roll; |
Rogercl | 5:d87c25f009d1 | 68 | *Pitch=Pitch_hat+ K(2,1)*Error_Yaw + K(2,2)*Error_Pitch +K(2,3)*Error_Roll; |
Rogercl | 5:d87c25f009d1 | 69 | *Roll=Roll_hat+ K(3,1)*Error_Yaw + K(3,2)*Error_Pitch +K(3,3)*Error_Roll; |
Rogercl | 5:d87c25f009d1 | 70 | |
Rogercl | 5:d87c25f009d1 | 71 | |
Rogercl | 5:d87c25f009d1 | 72 | //Calculando P |
Rogercl | 5:d87c25f009d1 | 73 | *P=-K*C*P_hat; |
Rogercl | 5:d87c25f009d1 | 74 | |
Rogercl | 5:d87c25f009d1 | 75 | } |