└── Plane_F4 └── EKF ├── EKF.cpp ├── EKF.h ├── ekf_capi.c └── ekf_capi.h /Plane_F4/EKF/EKF.cpp: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/wustyuyi/EKF/65d291b5fb3759ac9bc463b7b339a8c2a2c929b7/Plane_F4/EKF/EKF.cpp -------------------------------------------------------------------------------- /Plane_F4/EKF/EKF.h: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/wustyuyi/EKF/65d291b5fb3759ac9bc463b7b339a8c2a2c929b7/Plane_F4/EKF/EKF.h -------------------------------------------------------------------------------- /Plane_F4/EKF/ekf_capi.c: -------------------------------------------------------------------------------- 1 | /* 2 | * ekf_capi.c 3 | * 4 | * Created on: 2015年12月9日 5 | * Author: kohill 6 | */ 7 | 8 | 9 | 10 | 11 | /* 12 | * atitude.c 13 | * 14 | * Created on: 2015骞�8鏈�21鏃� 15 | * Author: yangkeshan 16 | */ 17 | 18 | //PB10,PB11 19 | //I2C 2 20 | #include "ekf_capi.h" 21 | 22 | static Quaternion cuQua = {0,0,0,0}; //当前四元数的值 23 | #define AcWeightFactor 0 24 | 25 | /* 26 | * @Function:取相反数 27 | */ 28 | inline static float o(float x) { 29 | return -1.0f * x; 30 | } 31 | 32 | /* 33 | * @Function:四元数到欧拉角 34 | */ 35 | static void quat2Angle(float quat[],float euler[]){ 36 | float r[5]; 37 | r[0] = -2 * (quat[1] * quat[2] - quat[0] * quat[3]); 38 | r[1] = quat[0] * quat[0] - quat[1] * quat[1] + quat[2] * quat[2]- quat[3] * quat[3]; 39 | r[2] = 2 * (quat[2] * quat[3] + quat[0] * quat[1]); 40 | r[3] = -2 * (quat[1] * quat[3] - quat[0] * quat[2]); 41 | r[4] = quat[0] * quat[0] - quat[1] * quat[1] - quat[2] * quat[2]+ quat[3] * quat[3]; 42 | euler[0] = atan2(r[0], r[1]) * 180 / PI; 43 | euler[1] = asin(r[2]) * 180 / PI; 44 | euler[2] = atan2(r[3], r[4]) * 180 / PI; 45 | } 46 | 47 | /* 48 | * @Function:由加速度传感器获得四元数 49 | */ 50 | static void getQuaterfromAccler(float quater[4]){ 51 | s16 Acce[3] = { 0,0,0 }; 52 | float Acceler[3] = {0,0,0}; 53 | float sum = 0; 54 | MPU_Get_Accelerometer(&Acce[0],&Acce[1],&Acce[2]); 55 | for(int i = 0;i<3;i++){ 56 | Acceler[i] = 1.0*Acce[i]/32767*2; //满量程4g，-32767-+32767 57 | sum+=Acceler[i]*Acceler[i]; 58 | } 59 | sum = sqrt(sum); 60 | for(int i = 0;i<3;i++){ 61 | Acceler[i] /= sum; 62 | } 63 | float pitch = asin(Acceler[1]); //求俯仰角 64 | float roll = atan(-Acceler[0]/Acceler[2]); //求横滚角 65 | //由加速度传感器无法得到偏航角 66 | pitch /=2; 67 | roll /=2; 68 | //得到四元数 69 | quater[0] = cos(pitch)*cos(roll); 70 | quater[1] = sin(pitch)*cos(roll); 71 | quater[2] = cos(pitch)*sin(roll); 72 | quater[3] = sin(pitch)*sin(roll); 73 | } 74 | /* 75 | * @Function: 更新四元数的值 76 | * @Parameter: x,y,z为当前时刻角速度 77 | */ 78 | void updateQuat(float x, float y, float z) { 79 | float exqu[4]; 80 | getQuaterfromAccler(exqu); 81 | 82 | float delteTheta[4][4]; 83 | float temp0 = 0; 84 | 85 | //获得角增量 86 | x *= SamplePeriod; 87 | y *= SamplePeriod; 88 | z *= SamplePeriod; 89 | 90 | temp0 = 1 - 0.125f * (x * x + y * y + z * z); 91 | 92 | x *= 0.5f; 93 | y *= 0.5f; 94 | z *= 0.5f; 95 | 96 | delteTheta[0][0] = temp0; 97 | delteTheta[0][1] = o(x); 98 | delteTheta[0][2] = o(y); 99 | delteTheta[0][3] = o(z); 100 | 101 | delteTheta[1][0] = x; 102 | delteTheta[1][1] = temp0; 103 | delteTheta[1][2] = z; 104 | delteTheta[1][3] = o(y); 105 | 106 | delteTheta[2][0] = y; 107 | delteTheta[2][1] = o(z); 108 | delteTheta[2][2] = temp0; 109 | delteTheta[2][3] = x; 110 | 111 | delteTheta[3][0] = z; 112 | delteTheta[3][1] = y; 113 | delteTheta[3][2] = o(x); 114 | delteTheta[3][3] = temp0; 115 | 116 | for (int i = 0; i < 4; i++) { 117 | cuQua[i] = delteTheta[i][0] * cuQua[0] + delteTheta[i][1] * cuQua[1] 118 | + delteTheta[i][2] * cuQua[2] + delteTheta[i][3] * cuQua[3]; 119 | } 120 | 121 | float sumsq; 122 | arm_sqrt_f32( 123 | cuQua[0] * cuQua[0] + cuQua[1] * cuQua[1] + cuQua[2] * cuQua[2] 124 | + cuQua[3] * cuQua[3], &sumsq); 125 | for (int i = 0; i < 4; i++) { 126 | cuQua[i] = cuQua[i] / sumsq; 127 | } 128 | for(int i = 0;i<4;i++){ 129 | cuQua[i] = AcWeightFactor*exqu[i] + (1-AcWeightFactor)*cuQua[i]; 130 | } 131 | float euler[3]; 132 | quat2Angle(cuQua,euler); 133 | } 134 | 135 | void initQua(){ 136 | getQuaterfromAccler(cuQua); 137 | getQuaterfromAccler(cuQua); 138 | getQuaterfromAccler(cuQua); 139 | } 140 | 141 | 142 | 143 | -------------------------------------------------------------------------------- /Plane_F4/EKF/ekf_capi.h: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/wustyuyi/EKF/65d291b5fb3759ac9bc463b7b339a8c2a2c929b7/Plane_F4/EKF/ekf_capi.h --------------------------------------------------------------------------------