Roger Lopes / Mbed 2 deprecated MARe_VT_ble_PeakSearch_Kalman_Inativ

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@6:e9a2bc040ada, 2019-08-04 (annotated)

Committer:
Rogercl
Date:
Sun Aug 04 11:38:08 2019 +0000
Revision:
6:e9a2bc040ada
Parent:
5:d87c25f009d1 
Algoritmo funcionando com a biblioteca de inatividade utilizando dos dados do acelerometro e a biblioteca de PeakSearch se utilizando dos dados filtrados pelo filtro Kalman.

Who changed what in which revision?

UserRevisionLine numberNew 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 }