ドローン用計測制御基板の作り方 vol.1 ハードウェア編 p.19掲載 カルマンフィルタによる姿勢推定
Dependencies: mbed MPU6050_alter
main.cpp@0:f41e4813b4c5, 2019-12-30 (annotated)
- Committer:
- Joeatsumi
- Date:
- Mon Dec 30 16:39:22 2019 +0000
- Revision:
- 0:f41e4813b4c5
20191231
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
Joeatsumi | 0:f41e4813b4c5 | 1 | //================================================== |
Joeatsumi | 0:f41e4813b4c5 | 2 | //Atitude estimation (kalman filter) |
Joeatsumi | 0:f41e4813b4c5 | 3 | //MPU board: mbed LPC1768 |
Joeatsumi | 0:f41e4813b4c5 | 4 | //Accelerometer +Gyro sensor : GY-521 |
Joeatsumi | 0:f41e4813b4c5 | 5 | // |
Joeatsumi | 0:f41e4813b4c5 | 6 | //Matrix functions were derived from トランジスタ技術2019年7月号 |
Joeatsumi | 0:f41e4813b4c5 | 7 | // |
Joeatsumi | 0:f41e4813b4c5 | 8 | //2019/12/30 A.Toda |
Joeatsumi | 0:f41e4813b4c5 | 9 | //================================================== |
Joeatsumi | 0:f41e4813b4c5 | 10 | #include "mbed.h" |
Joeatsumi | 0:f41e4813b4c5 | 11 | #include "MPU6050.h" |
Joeatsumi | 0:f41e4813b4c5 | 12 | |
Joeatsumi | 0:f41e4813b4c5 | 13 | //================================================== |
Joeatsumi | 0:f41e4813b4c5 | 14 | #define RAD_TO_DEG 57.2957795f // 180 / π |
Joeatsumi | 0:f41e4813b4c5 | 15 | #define MAX_MEAN_COUNTER 100 |
Joeatsumi | 0:f41e4813b4c5 | 16 | #define ACC_X 1.3//offset of x-axi accelerometer |
Joeatsumi | 0:f41e4813b4c5 | 17 | #define FRQ 100//Hz |
Joeatsumi | 0:f41e4813b4c5 | 18 | //================================================== |
Joeatsumi | 0:f41e4813b4c5 | 19 | |
Joeatsumi | 0:f41e4813b4c5 | 20 | DigitalOut led1(LED1); |
Joeatsumi | 0:f41e4813b4c5 | 21 | |
Joeatsumi | 0:f41e4813b4c5 | 22 | //Timer interrupt |
Joeatsumi | 0:f41e4813b4c5 | 23 | Ticker flipper; |
Joeatsumi | 0:f41e4813b4c5 | 24 | |
Joeatsumi | 0:f41e4813b4c5 | 25 | //Port Setting |
Joeatsumi | 0:f41e4813b4c5 | 26 | MPU6050 mpu(p9, p10); //Accelerometer + Gyro |
Joeatsumi | 0:f41e4813b4c5 | 27 | //(SDA,SCLK) |
Joeatsumi | 0:f41e4813b4c5 | 28 | |
Joeatsumi | 0:f41e4813b4c5 | 29 | Serial pc(USBTX, USBRX); //UART |
Joeatsumi | 0:f41e4813b4c5 | 30 | |
Joeatsumi | 0:f41e4813b4c5 | 31 | //================================================== |
Joeatsumi | 0:f41e4813b4c5 | 32 | //Accelerometer and gyro data |
Joeatsumi | 0:f41e4813b4c5 | 33 | //================================================== |
Joeatsumi | 0:f41e4813b4c5 | 34 | |
Joeatsumi | 0:f41e4813b4c5 | 35 | double acc[3]; //variables for accelerometer |
Joeatsumi | 0:f41e4813b4c5 | 36 | double gyro[3]; //variables for gyro |
Joeatsumi | 0:f41e4813b4c5 | 37 | |
Joeatsumi | 0:f41e4813b4c5 | 38 | double offset_gyro_x=0.0; |
Joeatsumi | 0:f41e4813b4c5 | 39 | double offset_gyro_y=0.0; |
Joeatsumi | 0:f41e4813b4c5 | 40 | |
Joeatsumi | 0:f41e4813b4c5 | 41 | double sum_gyro_x=0.0; |
Joeatsumi | 0:f41e4813b4c5 | 42 | double sum_gyro_y=0.0; |
Joeatsumi | 0:f41e4813b4c5 | 43 | |
Joeatsumi | 0:f41e4813b4c5 | 44 | //================================================== |
Joeatsumi | 0:f41e4813b4c5 | 45 | //Atitude data |
Joeatsumi | 0:f41e4813b4c5 | 46 | //================================================== |
Joeatsumi | 0:f41e4813b4c5 | 47 | double roll_and_pitch_acc[2][1];//atitude from accelerometer |
Joeatsumi | 0:f41e4813b4c5 | 48 | double roll_and_pitch[2][1]; //atitude from gyro and accelerometer |
Joeatsumi | 0:f41e4813b4c5 | 49 | |
Joeatsumi | 0:f41e4813b4c5 | 50 | double threshold_acc,threshold_acc_ini; |
Joeatsumi | 0:f41e4813b4c5 | 51 | |
Joeatsumi | 0:f41e4813b4c5 | 52 | //================================================== |
Joeatsumi | 0:f41e4813b4c5 | 53 | //Matrix definition |
Joeatsumi | 0:f41e4813b4c5 | 54 | //================================================== |
Joeatsumi | 0:f41e4813b4c5 | 55 | double MAT_A[2][2]; |
Joeatsumi | 0:f41e4813b4c5 | 56 | double inv_MAT_A[2][2]; |
Joeatsumi | 0:f41e4813b4c5 | 57 | |
Joeatsumi | 0:f41e4813b4c5 | 58 | double MAT_B[2][2]; |
Joeatsumi | 0:f41e4813b4c5 | 59 | double inv_MAT_B[2][2]; |
Joeatsumi | 0:f41e4813b4c5 | 60 | |
Joeatsumi | 0:f41e4813b4c5 | 61 | double MAT_C[2][2]; |
Joeatsumi | 0:f41e4813b4c5 | 62 | double inv_MAT_C[2][2]; |
Joeatsumi | 0:f41e4813b4c5 | 63 | |
Joeatsumi | 0:f41e4813b4c5 | 64 | double MAT_X_k[2][1]; |
Joeatsumi | 0:f41e4813b4c5 | 65 | double pre_MAT_X_k[2][1]; |
Joeatsumi | 0:f41e4813b4c5 | 66 | |
Joeatsumi | 0:f41e4813b4c5 | 67 | double COV_P[2][2];//covariance matrix of atitude angles |
Joeatsumi | 0:f41e4813b4c5 | 68 | double pre_COV_P[2][2];//covariance matrix of atitude angles |
Joeatsumi | 0:f41e4813b4c5 | 69 | double pre_COV_P_2[2][2];//covariance matrix of atitude angles |
Joeatsumi | 0:f41e4813b4c5 | 70 | double pre_COV_P_3[2][2];//covariance matrix of atitude angles |
Joeatsumi | 0:f41e4813b4c5 | 71 | |
Joeatsumi | 0:f41e4813b4c5 | 72 | double COV_Q[2][2];//covariance matrix of gyro inputs |
Joeatsumi | 0:f41e4813b4c5 | 73 | double COV_R[2][2];//covariance matrix of atitude angles from accelerometer |
Joeatsumi | 0:f41e4813b4c5 | 74 | |
Joeatsumi | 0:f41e4813b4c5 | 75 | //========================================================= |
Joeatsumi | 0:f41e4813b4c5 | 76 | // Matrix common functions |
Joeatsumi | 0:f41e4813b4c5 | 77 | //========================================================= |
Joeatsumi | 0:f41e4813b4c5 | 78 | //Matrix addition |
Joeatsumi | 0:f41e4813b4c5 | 79 | void mat_add(double *m1, double *m2, double *sol, int row, int column) |
Joeatsumi | 0:f41e4813b4c5 | 80 | { |
Joeatsumi | 0:f41e4813b4c5 | 81 | for(int i=0; i<row; i++) |
Joeatsumi | 0:f41e4813b4c5 | 82 | { |
Joeatsumi | 0:f41e4813b4c5 | 83 | for(int j=0; j<column; j++) |
Joeatsumi | 0:f41e4813b4c5 | 84 | { |
Joeatsumi | 0:f41e4813b4c5 | 85 | sol[i*column + j] = m1[i*column + j] + m2[i*column + j]; |
Joeatsumi | 0:f41e4813b4c5 | 86 | } |
Joeatsumi | 0:f41e4813b4c5 | 87 | } |
Joeatsumi | 0:f41e4813b4c5 | 88 | return; |
Joeatsumi | 0:f41e4813b4c5 | 89 | } |
Joeatsumi | 0:f41e4813b4c5 | 90 | |
Joeatsumi | 0:f41e4813b4c5 | 91 | //Matrix subtraction |
Joeatsumi | 0:f41e4813b4c5 | 92 | void mat_sub(double *m1, double *m2, double *sol, int row, int column) |
Joeatsumi | 0:f41e4813b4c5 | 93 | { |
Joeatsumi | 0:f41e4813b4c5 | 94 | for(int i=0; i<row; i++) |
Joeatsumi | 0:f41e4813b4c5 | 95 | { |
Joeatsumi | 0:f41e4813b4c5 | 96 | for(int j=0; j<column; j++) |
Joeatsumi | 0:f41e4813b4c5 | 97 | { |
Joeatsumi | 0:f41e4813b4c5 | 98 | sol[i*column + j] = m1[i*column + j] - m2[i*column + j]; |
Joeatsumi | 0:f41e4813b4c5 | 99 | } |
Joeatsumi | 0:f41e4813b4c5 | 100 | } |
Joeatsumi | 0:f41e4813b4c5 | 101 | return; |
Joeatsumi | 0:f41e4813b4c5 | 102 | } |
Joeatsumi | 0:f41e4813b4c5 | 103 | |
Joeatsumi | 0:f41e4813b4c5 | 104 | //Matrix multiplication |
Joeatsumi | 0:f41e4813b4c5 | 105 | void mat_mul(double *m1, double *m2, double *sol, int row1, int column1, int row2, int column2) |
Joeatsumi | 0:f41e4813b4c5 | 106 | { |
Joeatsumi | 0:f41e4813b4c5 | 107 | for(int i=0; i<row1; i++) |
Joeatsumi | 0:f41e4813b4c5 | 108 | { |
Joeatsumi | 0:f41e4813b4c5 | 109 | for(int j=0; j<column2; j++) |
Joeatsumi | 0:f41e4813b4c5 | 110 | { |
Joeatsumi | 0:f41e4813b4c5 | 111 | sol[i*column2 + j] = 0; |
Joeatsumi | 0:f41e4813b4c5 | 112 | for(int k=0; k<column1; k++) |
Joeatsumi | 0:f41e4813b4c5 | 113 | { |
Joeatsumi | 0:f41e4813b4c5 | 114 | sol[i*column2 + j] += m1[i*column1 + k]*m2[k*column2 + j]; |
Joeatsumi | 0:f41e4813b4c5 | 115 | } |
Joeatsumi | 0:f41e4813b4c5 | 116 | } |
Joeatsumi | 0:f41e4813b4c5 | 117 | } |
Joeatsumi | 0:f41e4813b4c5 | 118 | return; |
Joeatsumi | 0:f41e4813b4c5 | 119 | } |
Joeatsumi | 0:f41e4813b4c5 | 120 | |
Joeatsumi | 0:f41e4813b4c5 | 121 | //Matrix transposition |
Joeatsumi | 0:f41e4813b4c5 | 122 | void mat_tran(double *m1, double *sol, int row_original, int column_original) |
Joeatsumi | 0:f41e4813b4c5 | 123 | { |
Joeatsumi | 0:f41e4813b4c5 | 124 | for(int i=0; i<row_original; i++) |
Joeatsumi | 0:f41e4813b4c5 | 125 | { |
Joeatsumi | 0:f41e4813b4c5 | 126 | for(int j=0; j<column_original; j++) |
Joeatsumi | 0:f41e4813b4c5 | 127 | { |
Joeatsumi | 0:f41e4813b4c5 | 128 | sol[j*row_original + i] = m1[i*column_original + j]; |
Joeatsumi | 0:f41e4813b4c5 | 129 | } |
Joeatsumi | 0:f41e4813b4c5 | 130 | } |
Joeatsumi | 0:f41e4813b4c5 | 131 | return; |
Joeatsumi | 0:f41e4813b4c5 | 132 | } |
Joeatsumi | 0:f41e4813b4c5 | 133 | |
Joeatsumi | 0:f41e4813b4c5 | 134 | //Matrix scalar maltiplication |
Joeatsumi | 0:f41e4813b4c5 | 135 | void mat_mul_const(double *m1,double c, double *sol, int row, int column) |
Joeatsumi | 0:f41e4813b4c5 | 136 | { |
Joeatsumi | 0:f41e4813b4c5 | 137 | for(int i=0; i<row; i++) |
Joeatsumi | 0:f41e4813b4c5 | 138 | { |
Joeatsumi | 0:f41e4813b4c5 | 139 | for(int j=0; j<column; j++) |
Joeatsumi | 0:f41e4813b4c5 | 140 | { |
Joeatsumi | 0:f41e4813b4c5 | 141 | sol[i*column + j] = c * m1[i*column + j]; |
Joeatsumi | 0:f41e4813b4c5 | 142 | } |
Joeatsumi | 0:f41e4813b4c5 | 143 | } |
Joeatsumi | 0:f41e4813b4c5 | 144 | return; |
Joeatsumi | 0:f41e4813b4c5 | 145 | } |
Joeatsumi | 0:f41e4813b4c5 | 146 | |
Joeatsumi | 0:f41e4813b4c5 | 147 | //Matrix inversion (by Gaussian elimination) |
Joeatsumi | 0:f41e4813b4c5 | 148 | void mat_inv(double *m, double *sol, int column, int row) |
Joeatsumi | 0:f41e4813b4c5 | 149 | { |
Joeatsumi | 0:f41e4813b4c5 | 150 | //allocate memory for a temporary matrix |
Joeatsumi | 0:f41e4813b4c5 | 151 | double* temp = (double *)malloc( column*2*row*sizeof(double) ); |
Joeatsumi | 0:f41e4813b4c5 | 152 | |
Joeatsumi | 0:f41e4813b4c5 | 153 | //make the augmented matrix |
Joeatsumi | 0:f41e4813b4c5 | 154 | for(int i=0; i<column; i++) |
Joeatsumi | 0:f41e4813b4c5 | 155 | { |
Joeatsumi | 0:f41e4813b4c5 | 156 | //copy original matrix |
Joeatsumi | 0:f41e4813b4c5 | 157 | for(int j=0; j<row; j++) |
Joeatsumi | 0:f41e4813b4c5 | 158 | { |
Joeatsumi | 0:f41e4813b4c5 | 159 | temp[i*(2*row) + j] = m[i*row + j]; |
Joeatsumi | 0:f41e4813b4c5 | 160 | } |
Joeatsumi | 0:f41e4813b4c5 | 161 | |
Joeatsumi | 0:f41e4813b4c5 | 162 | //make identity matrix |
Joeatsumi | 0:f41e4813b4c5 | 163 | for(int j=row; j<row*2; j++) |
Joeatsumi | 0:f41e4813b4c5 | 164 | { |
Joeatsumi | 0:f41e4813b4c5 | 165 | if(j-row == i) |
Joeatsumi | 0:f41e4813b4c5 | 166 | { |
Joeatsumi | 0:f41e4813b4c5 | 167 | temp[i*(2*row) + j] = 1; |
Joeatsumi | 0:f41e4813b4c5 | 168 | } |
Joeatsumi | 0:f41e4813b4c5 | 169 | else |
Joeatsumi | 0:f41e4813b4c5 | 170 | { |
Joeatsumi | 0:f41e4813b4c5 | 171 | temp[i*(2*row) + j] = 0; |
Joeatsumi | 0:f41e4813b4c5 | 172 | } |
Joeatsumi | 0:f41e4813b4c5 | 173 | } |
Joeatsumi | 0:f41e4813b4c5 | 174 | } |
Joeatsumi | 0:f41e4813b4c5 | 175 | |
Joeatsumi | 0:f41e4813b4c5 | 176 | //Sweep (down) |
Joeatsumi | 0:f41e4813b4c5 | 177 | for(int i=0; i<column; i++) |
Joeatsumi | 0:f41e4813b4c5 | 178 | { |
Joeatsumi | 0:f41e4813b4c5 | 179 | //pivot selection |
Joeatsumi | 0:f41e4813b4c5 | 180 | double pivot = temp[i*(2*row) + i]; |
Joeatsumi | 0:f41e4813b4c5 | 181 | int pivot_index = i; |
Joeatsumi | 0:f41e4813b4c5 | 182 | double pivot_temp; |
Joeatsumi | 0:f41e4813b4c5 | 183 | for(int j=i; j<column;j++) |
Joeatsumi | 0:f41e4813b4c5 | 184 | { |
Joeatsumi | 0:f41e4813b4c5 | 185 | if( temp[j*(2*row)+i] > pivot ) |
Joeatsumi | 0:f41e4813b4c5 | 186 | { |
Joeatsumi | 0:f41e4813b4c5 | 187 | pivot = temp[j*(2*row) + i]; |
Joeatsumi | 0:f41e4813b4c5 | 188 | pivot_index = j; |
Joeatsumi | 0:f41e4813b4c5 | 189 | } |
Joeatsumi | 0:f41e4813b4c5 | 190 | } |
Joeatsumi | 0:f41e4813b4c5 | 191 | if(pivot_index != i) |
Joeatsumi | 0:f41e4813b4c5 | 192 | { |
Joeatsumi | 0:f41e4813b4c5 | 193 | for(int j=0; j<2*row; j++) |
Joeatsumi | 0:f41e4813b4c5 | 194 | { |
Joeatsumi | 0:f41e4813b4c5 | 195 | pivot_temp = temp[ pivot_index * (2*row) + j ]; |
Joeatsumi | 0:f41e4813b4c5 | 196 | temp[pivot_index * (2*row) + j] = temp[i*(2*row) + j]; |
Joeatsumi | 0:f41e4813b4c5 | 197 | temp[i*(2*row) + j] = pivot_temp; |
Joeatsumi | 0:f41e4813b4c5 | 198 | } |
Joeatsumi | 0:f41e4813b4c5 | 199 | } |
Joeatsumi | 0:f41e4813b4c5 | 200 | |
Joeatsumi | 0:f41e4813b4c5 | 201 | //division |
Joeatsumi | 0:f41e4813b4c5 | 202 | for(int j=0; j<2*row; j++) |
Joeatsumi | 0:f41e4813b4c5 | 203 | { |
Joeatsumi | 0:f41e4813b4c5 | 204 | temp[i*(2*row) + j] /= pivot; |
Joeatsumi | 0:f41e4813b4c5 | 205 | } |
Joeatsumi | 0:f41e4813b4c5 | 206 | |
Joeatsumi | 0:f41e4813b4c5 | 207 | //sweep |
Joeatsumi | 0:f41e4813b4c5 | 208 | for(int j=i+1; j<column; j++) |
Joeatsumi | 0:f41e4813b4c5 | 209 | { |
Joeatsumi | 0:f41e4813b4c5 | 210 | double temp2 = temp[j*(2*row) + i]; |
Joeatsumi | 0:f41e4813b4c5 | 211 | |
Joeatsumi | 0:f41e4813b4c5 | 212 | //sweep each row |
Joeatsumi | 0:f41e4813b4c5 | 213 | for(int k=0; k<row*2; k++) |
Joeatsumi | 0:f41e4813b4c5 | 214 | { |
Joeatsumi | 0:f41e4813b4c5 | 215 | temp[j*(2*row) + k] -= temp2 * temp[ i*(2*row) + k ]; |
Joeatsumi | 0:f41e4813b4c5 | 216 | } |
Joeatsumi | 0:f41e4813b4c5 | 217 | } |
Joeatsumi | 0:f41e4813b4c5 | 218 | } |
Joeatsumi | 0:f41e4813b4c5 | 219 | |
Joeatsumi | 0:f41e4813b4c5 | 220 | //Sweep (up) |
Joeatsumi | 0:f41e4813b4c5 | 221 | for(int i=0; i<column-1; i++) |
Joeatsumi | 0:f41e4813b4c5 | 222 | { |
Joeatsumi | 0:f41e4813b4c5 | 223 | for(int j=i+1; j<column; j++) |
Joeatsumi | 0:f41e4813b4c5 | 224 | { |
Joeatsumi | 0:f41e4813b4c5 | 225 | double pivot = temp[ (column-1-j)*(2*row) + (row-1-i)]; |
Joeatsumi | 0:f41e4813b4c5 | 226 | for(int k=0; k<2*row; k++) |
Joeatsumi | 0:f41e4813b4c5 | 227 | { |
Joeatsumi | 0:f41e4813b4c5 | 228 | temp[(column-1-j)*(2*row)+k] -= pivot * temp[(column-1-i)*(2*row)+k]; |
Joeatsumi | 0:f41e4813b4c5 | 229 | } |
Joeatsumi | 0:f41e4813b4c5 | 230 | } |
Joeatsumi | 0:f41e4813b4c5 | 231 | } |
Joeatsumi | 0:f41e4813b4c5 | 232 | |
Joeatsumi | 0:f41e4813b4c5 | 233 | //copy result |
Joeatsumi | 0:f41e4813b4c5 | 234 | for(int i=0; i<column; i++) |
Joeatsumi | 0:f41e4813b4c5 | 235 | { |
Joeatsumi | 0:f41e4813b4c5 | 236 | for(int j=0; j<row; j++) |
Joeatsumi | 0:f41e4813b4c5 | 237 | { |
Joeatsumi | 0:f41e4813b4c5 | 238 | sol[i*row + j] = temp[i*(2*row) + (j+row)]; |
Joeatsumi | 0:f41e4813b4c5 | 239 | } |
Joeatsumi | 0:f41e4813b4c5 | 240 | } |
Joeatsumi | 0:f41e4813b4c5 | 241 | free(temp); |
Joeatsumi | 0:f41e4813b4c5 | 242 | return; |
Joeatsumi | 0:f41e4813b4c5 | 243 | } |
Joeatsumi | 0:f41e4813b4c5 | 244 | |
Joeatsumi | 0:f41e4813b4c5 | 245 | //================================================== |
Joeatsumi | 0:f41e4813b4c5 | 246 | //Gyro and accelerometer functions |
Joeatsumi | 0:f41e4813b4c5 | 247 | //================================================== |
Joeatsumi | 0:f41e4813b4c5 | 248 | //get data |
Joeatsumi | 0:f41e4813b4c5 | 249 | void aquisition_sensor_values(double *a,double *g){ |
Joeatsumi | 0:f41e4813b4c5 | 250 | |
Joeatsumi | 0:f41e4813b4c5 | 251 | float ac[3],gy[3]; |
Joeatsumi | 0:f41e4813b4c5 | 252 | |
Joeatsumi | 0:f41e4813b4c5 | 253 | mpu.getAccelero(ac);//get acceleration (Accelerometer) |
Joeatsumi | 0:f41e4813b4c5 | 254 | //x_axis acc[0] |
Joeatsumi | 0:f41e4813b4c5 | 255 | //y_axis acc[1] |
Joeatsumi | 0:f41e4813b4c5 | 256 | //z_axis acc[2] |
Joeatsumi | 0:f41e4813b4c5 | 257 | mpu.getGyro(gy); //get rate of angle(Gyro) |
Joeatsumi | 0:f41e4813b4c5 | 258 | //x_axis gyro[0] |
Joeatsumi | 0:f41e4813b4c5 | 259 | //y_axis gyro[1] |
Joeatsumi | 0:f41e4813b4c5 | 260 | //z_axis gyro[2] |
Joeatsumi | 0:f41e4813b4c5 | 261 | |
Joeatsumi | 0:f41e4813b4c5 | 262 | //Invertion for direction of Accelerometer axis |
Joeatsumi | 0:f41e4813b4c5 | 263 | ac[0]*=(-1.0); |
Joeatsumi | 0:f41e4813b4c5 | 264 | ac[0]+=ACC_X; |
Joeatsumi | 0:f41e4813b4c5 | 265 | |
Joeatsumi | 0:f41e4813b4c5 | 266 | ac[2]*=(-1.0); |
Joeatsumi | 0:f41e4813b4c5 | 267 | |
Joeatsumi | 0:f41e4813b4c5 | 268 | //Unit convertion of rate of angle(radian to degree) |
Joeatsumi | 0:f41e4813b4c5 | 269 | gy[0]*=RAD_TO_DEG; |
Joeatsumi | 0:f41e4813b4c5 | 270 | gy[0]*=(-1.0); |
Joeatsumi | 0:f41e4813b4c5 | 271 | |
Joeatsumi | 0:f41e4813b4c5 | 272 | gy[1]*=RAD_TO_DEG; |
Joeatsumi | 0:f41e4813b4c5 | 273 | |
Joeatsumi | 0:f41e4813b4c5 | 274 | gy[2]*=RAD_TO_DEG; |
Joeatsumi | 0:f41e4813b4c5 | 275 | gy[2]*=(-1.0); |
Joeatsumi | 0:f41e4813b4c5 | 276 | |
Joeatsumi | 0:f41e4813b4c5 | 277 | for(int i=0;i<3;i++){ |
Joeatsumi | 0:f41e4813b4c5 | 278 | a[i]=double(ac[i]); |
Joeatsumi | 0:f41e4813b4c5 | 279 | g[i]=double(gy[i]); |
Joeatsumi | 0:f41e4813b4c5 | 280 | } |
Joeatsumi | 0:f41e4813b4c5 | 281 | g[0]-=offset_gyro_x;//offset rejection |
Joeatsumi | 0:f41e4813b4c5 | 282 | g[1]-=offset_gyro_y;//offset rejection |
Joeatsumi | 0:f41e4813b4c5 | 283 | |
Joeatsumi | 0:f41e4813b4c5 | 284 | return; |
Joeatsumi | 0:f41e4813b4c5 | 285 | |
Joeatsumi | 0:f41e4813b4c5 | 286 | } |
Joeatsumi | 0:f41e4813b4c5 | 287 | |
Joeatsumi | 0:f41e4813b4c5 | 288 | //calculate offset of gyro |
Joeatsumi | 0:f41e4813b4c5 | 289 | void offset_calculation_for_gyro(){ |
Joeatsumi | 0:f41e4813b4c5 | 290 | |
Joeatsumi | 0:f41e4813b4c5 | 291 | //Accelerometer and gyro setting |
Joeatsumi | 0:f41e4813b4c5 | 292 | mpu.setAcceleroRange(0);//acceleration range is +-2G |
Joeatsumi | 0:f41e4813b4c5 | 293 | mpu.setGyroRange(1);//gyro rate is +-500degree per second(dps) |
Joeatsumi | 0:f41e4813b4c5 | 294 | |
Joeatsumi | 0:f41e4813b4c5 | 295 | //calculate offset of gyro |
Joeatsumi | 0:f41e4813b4c5 | 296 | for(int mean_counter=0; mean_counter<MAX_MEAN_COUNTER ;mean_counter++){ |
Joeatsumi | 0:f41e4813b4c5 | 297 | aquisition_sensor_values(acc,gyro); |
Joeatsumi | 0:f41e4813b4c5 | 298 | sum_gyro_x+=gyro[0]; |
Joeatsumi | 0:f41e4813b4c5 | 299 | sum_gyro_y+=gyro[1]; |
Joeatsumi | 0:f41e4813b4c5 | 300 | wait(0.01); |
Joeatsumi | 0:f41e4813b4c5 | 301 | } |
Joeatsumi | 0:f41e4813b4c5 | 302 | |
Joeatsumi | 0:f41e4813b4c5 | 303 | offset_gyro_x=sum_gyro_x/MAX_MEAN_COUNTER; |
Joeatsumi | 0:f41e4813b4c5 | 304 | offset_gyro_y=sum_gyro_y/MAX_MEAN_COUNTER; |
Joeatsumi | 0:f41e4813b4c5 | 305 | |
Joeatsumi | 0:f41e4813b4c5 | 306 | return; |
Joeatsumi | 0:f41e4813b4c5 | 307 | } |
Joeatsumi | 0:f41e4813b4c5 | 308 | |
Joeatsumi | 0:f41e4813b4c5 | 309 | //atitude calculation from acceleromter |
Joeatsumi | 0:f41e4813b4c5 | 310 | void atitude_estimation_from_accelerometer(double *a,double *roll_and_pitch){ |
Joeatsumi | 0:f41e4813b4c5 | 311 | |
Joeatsumi | 0:f41e4813b4c5 | 312 | //roll_and_pitch[0] = atan2(a[1], a[2])*RAD_TO_DEG;//roll |
Joeatsumi | 0:f41e4813b4c5 | 313 | roll_and_pitch[0] = atan(a[1]/a[2])*RAD_TO_DEG;//roll |
Joeatsumi | 0:f41e4813b4c5 | 314 | //roll_and_pitch[1] = atan(a[0]/sqrt( a[1]*a[1]+a[2]*a[2] ) )*RAD_TO_DEG;//pitch |
Joeatsumi | 0:f41e4813b4c5 | 315 | roll_and_pitch[1] = atan2(a[0],sqrt( a[1]*a[1]+a[2]*a[2] ) )*RAD_TO_DEG;//pitch |
Joeatsumi | 0:f41e4813b4c5 | 316 | |
Joeatsumi | 0:f41e4813b4c5 | 317 | return; |
Joeatsumi | 0:f41e4813b4c5 | 318 | } |
Joeatsumi | 0:f41e4813b4c5 | 319 | |
Joeatsumi | 0:f41e4813b4c5 | 320 | //atitude calculation |
Joeatsumi | 0:f41e4813b4c5 | 321 | void atitude_update(){ |
Joeatsumi | 0:f41e4813b4c5 | 322 | |
Joeatsumi | 0:f41e4813b4c5 | 323 | |
Joeatsumi | 0:f41e4813b4c5 | 324 | aquisition_sensor_values(acc,gyro);//加速度、ジャイロの取得 |
Joeatsumi | 0:f41e4813b4c5 | 325 | |
Joeatsumi | 0:f41e4813b4c5 | 326 | //入力の設定 |
Joeatsumi | 0:f41e4813b4c5 | 327 | double MAT_IN[2][1]={}; |
Joeatsumi | 0:f41e4813b4c5 | 328 | double pre_MAT_IN[2][1]={}; |
Joeatsumi | 0:f41e4813b4c5 | 329 | |
Joeatsumi | 0:f41e4813b4c5 | 330 | MAT_IN[0][0]=gyro[0]; |
Joeatsumi | 0:f41e4813b4c5 | 331 | MAT_IN[1][0]=gyro[1]; |
Joeatsumi | 0:f41e4813b4c5 | 332 | |
Joeatsumi | 0:f41e4813b4c5 | 333 | /*--------------------Time update phase 時間更新フェーズ--------------------*/ |
Joeatsumi | 0:f41e4813b4c5 | 334 | //A*x+B*u |
Joeatsumi | 0:f41e4813b4c5 | 335 | |
Joeatsumi | 0:f41e4813b4c5 | 336 | mat_mul(MAT_A[0], MAT_X_k[0],pre_MAT_X_k[0] ,2, 2, 2, 1); |
Joeatsumi | 0:f41e4813b4c5 | 337 | mat_mul(MAT_B[0], MAT_IN[0],pre_MAT_IN[0] ,2, 2, 2, 1); |
Joeatsumi | 0:f41e4813b4c5 | 338 | |
Joeatsumi | 0:f41e4813b4c5 | 339 | mat_add(pre_MAT_X_k[0],pre_MAT_IN[0], pre_MAT_X_k[0], 2, 1); |
Joeatsumi | 0:f41e4813b4c5 | 340 | |
Joeatsumi | 0:f41e4813b4c5 | 341 | //A*P*A^T+B*P*B^T |
Joeatsumi | 0:f41e4813b4c5 | 342 | double pre_COV_P_1[2][2]={}; |
Joeatsumi | 0:f41e4813b4c5 | 343 | double pre_COV_P_1_2[2][2]={}; |
Joeatsumi | 0:f41e4813b4c5 | 344 | |
Joeatsumi | 0:f41e4813b4c5 | 345 | double pre_COV_P_2[2][2]={}; |
Joeatsumi | 0:f41e4813b4c5 | 346 | double pre_COV_P_2_2[2][2]={}; |
Joeatsumi | 0:f41e4813b4c5 | 347 | |
Joeatsumi | 0:f41e4813b4c5 | 348 | mat_mul(MAT_A[0], COV_P[0],pre_COV_P_1[0] ,2, 2, 2, 2); |
Joeatsumi | 0:f41e4813b4c5 | 349 | mat_mul(pre_COV_P_1[0], inv_MAT_A[0] , pre_COV_P_1_2[0] ,2, 2, 2, 2); |
Joeatsumi | 0:f41e4813b4c5 | 350 | |
Joeatsumi | 0:f41e4813b4c5 | 351 | |
Joeatsumi | 0:f41e4813b4c5 | 352 | mat_mul(MAT_B[0], COV_Q[0],pre_COV_P_2[0] ,2, 2, 2, 2); |
Joeatsumi | 0:f41e4813b4c5 | 353 | mat_mul(pre_COV_P_2[0], inv_MAT_B[0] , pre_COV_P_2_2[0] ,2, 2, 2, 2); |
Joeatsumi | 0:f41e4813b4c5 | 354 | |
Joeatsumi | 0:f41e4813b4c5 | 355 | mat_add(pre_COV_P_1_2[0],pre_COV_P_2_2[0], pre_COV_P[0], 2, 2); |
Joeatsumi | 0:f41e4813b4c5 | 356 | |
Joeatsumi | 0:f41e4813b4c5 | 357 | //pc.printf("1;Roll=%f,Pitch=%f\r\n",pre_COV_P[0][0],pre_COV_P[1][1]); |
Joeatsumi | 0:f41e4813b4c5 | 358 | /*------------------------------------------------------------------------*/ |
Joeatsumi | 0:f41e4813b4c5 | 359 | |
Joeatsumi | 0:f41e4813b4c5 | 360 | /*--------------------Measurement update phase 観測更新フェーズ--------------------*/ |
Joeatsumi | 0:f41e4813b4c5 | 361 | |
Joeatsumi | 0:f41e4813b4c5 | 362 | threshold_acc=sqrt(acc[0]*acc[0]+acc[1]*acc[1]+acc[2]*acc[2]); |
Joeatsumi | 0:f41e4813b4c5 | 363 | |
Joeatsumi | 0:f41e4813b4c5 | 364 | //加速度のノルムが9.8m/s^2付近かどうかの判定 |
Joeatsumi | 0:f41e4813b4c5 | 365 | if((threshold_acc>=0.95*threshold_acc_ini)&&(threshold_acc<=1.05*threshold_acc_ini)){ |
Joeatsumi | 0:f41e4813b4c5 | 366 | |
Joeatsumi | 0:f41e4813b4c5 | 367 | //pc.printf("corrected\r\n"); |
Joeatsumi | 0:f41e4813b4c5 | 368 | led1=0; |
Joeatsumi | 0:f41e4813b4c5 | 369 | |
Joeatsumi | 0:f41e4813b4c5 | 370 | double GAIN_K[2][2]={}; |
Joeatsumi | 0:f41e4813b4c5 | 371 | double pre_GAIN_K_1[2][2]={}; |
Joeatsumi | 0:f41e4813b4c5 | 372 | double pre_GAIN_K_1_2[2][2]={}; |
Joeatsumi | 0:f41e4813b4c5 | 373 | double pre_GAIN_K_1_3[2][2]={}; |
Joeatsumi | 0:f41e4813b4c5 | 374 | double pre_GAIN_K_1_4[2][2]={}; |
Joeatsumi | 0:f41e4813b4c5 | 375 | |
Joeatsumi | 0:f41e4813b4c5 | 376 | double pre_GAIN_K_2[2][2]={}; |
Joeatsumi | 0:f41e4813b4c5 | 377 | |
Joeatsumi | 0:f41e4813b4c5 | 378 | //K=P*C^T*(C*P*C^T+R)^-1 |
Joeatsumi | 0:f41e4813b4c5 | 379 | //(C*P*C^T+R)^-1 |
Joeatsumi | 0:f41e4813b4c5 | 380 | mat_mul(MAT_C[0], pre_COV_P[0] , pre_GAIN_K_1[0] ,2, 2, 2, 2); |
Joeatsumi | 0:f41e4813b4c5 | 381 | mat_mul(pre_GAIN_K_1[0], inv_MAT_C[0] , pre_GAIN_K_1_2[0] ,2, 2, 2, 2); |
Joeatsumi | 0:f41e4813b4c5 | 382 | |
Joeatsumi | 0:f41e4813b4c5 | 383 | mat_add(pre_GAIN_K_1_2[0],COV_R[0], pre_GAIN_K_1_3[0], 2, 2); |
Joeatsumi | 0:f41e4813b4c5 | 384 | |
Joeatsumi | 0:f41e4813b4c5 | 385 | mat_inv(pre_GAIN_K_1_3[0], pre_GAIN_K_1_4[0], 2, 2); |
Joeatsumi | 0:f41e4813b4c5 | 386 | |
Joeatsumi | 0:f41e4813b4c5 | 387 | //P*C^T |
Joeatsumi | 0:f41e4813b4c5 | 388 | mat_mul(pre_COV_P[0], inv_MAT_C[0] , pre_GAIN_K_2[0] ,2, 2, 2, 2); |
Joeatsumi | 0:f41e4813b4c5 | 389 | |
Joeatsumi | 0:f41e4813b4c5 | 390 | //P*C^T*(C*P*C^T+R)^-1 |
Joeatsumi | 0:f41e4813b4c5 | 391 | mat_mul(pre_GAIN_K_2[0], pre_GAIN_K_1_4[0] , GAIN_K[0] ,2, 2, 2, 2); |
Joeatsumi | 0:f41e4813b4c5 | 392 | |
Joeatsumi | 0:f41e4813b4c5 | 393 | double pre_MAT_X_k_2[2][1]={}; |
Joeatsumi | 0:f41e4813b4c5 | 394 | double pre_MAT_X_k_2_2[2][1]={}; |
Joeatsumi | 0:f41e4813b4c5 | 395 | |
Joeatsumi | 0:f41e4813b4c5 | 396 | //X=X+K(y-C*X) |
Joeatsumi | 0:f41e4813b4c5 | 397 | |
Joeatsumi | 0:f41e4813b4c5 | 398 | atitude_estimation_from_accelerometer(acc,roll_and_pitch_acc[0]); |
Joeatsumi | 0:f41e4813b4c5 | 399 | mat_sub(roll_and_pitch_acc[0], pre_MAT_X_k[0], pre_MAT_X_k_2[0], 2, 1); |
Joeatsumi | 0:f41e4813b4c5 | 400 | |
Joeatsumi | 0:f41e4813b4c5 | 401 | mat_mul(GAIN_K[0], pre_MAT_X_k_2[0] ,pre_MAT_X_k_2_2[0] ,2, 2, 2, 1); |
Joeatsumi | 0:f41e4813b4c5 | 402 | |
Joeatsumi | 0:f41e4813b4c5 | 403 | mat_add(pre_MAT_X_k[0], pre_MAT_X_k_2_2[0], MAT_X_k[0], 2, 1); |
Joeatsumi | 0:f41e4813b4c5 | 404 | |
Joeatsumi | 0:f41e4813b4c5 | 405 | //P=(I-K*C)*P |
Joeatsumi | 0:f41e4813b4c5 | 406 | double MAT_UNIT[2][2]={}; |
Joeatsumi | 0:f41e4813b4c5 | 407 | /*initialize Unit matrix*/ |
Joeatsumi | 0:f41e4813b4c5 | 408 | MAT_UNIT[0][0]=1.0; |
Joeatsumi | 0:f41e4813b4c5 | 409 | MAT_UNIT[0][1]=0.0; |
Joeatsumi | 0:f41e4813b4c5 | 410 | MAT_UNIT[1][0]=0.0; |
Joeatsumi | 0:f41e4813b4c5 | 411 | MAT_UNIT[1][1]=1.0; |
Joeatsumi | 0:f41e4813b4c5 | 412 | /*----------------------*/ |
Joeatsumi | 0:f41e4813b4c5 | 413 | |
Joeatsumi | 0:f41e4813b4c5 | 414 | mat_sub(MAT_UNIT[0], GAIN_K[0], pre_COV_P_3[0], 2, 2); |
Joeatsumi | 0:f41e4813b4c5 | 415 | mat_mul(pre_COV_P_3[0], pre_COV_P[0], COV_P[0], 2, 2, 2, 2); |
Joeatsumi | 0:f41e4813b4c5 | 416 | |
Joeatsumi | 0:f41e4813b4c5 | 417 | |
Joeatsumi | 0:f41e4813b4c5 | 418 | }else{ |
Joeatsumi | 0:f41e4813b4c5 | 419 | //pc.printf("Not corrected\r\n"); |
Joeatsumi | 0:f41e4813b4c5 | 420 | led1=1; |
Joeatsumi | 0:f41e4813b4c5 | 421 | |
Joeatsumi | 0:f41e4813b4c5 | 422 | MAT_X_k[0][0]=pre_MAT_X_k[0][0]; |
Joeatsumi | 0:f41e4813b4c5 | 423 | MAT_X_k[1][0]=pre_MAT_X_k[1][0]; |
Joeatsumi | 0:f41e4813b4c5 | 424 | |
Joeatsumi | 0:f41e4813b4c5 | 425 | COV_P[0][0]=pre_COV_P[0][0]; |
Joeatsumi | 0:f41e4813b4c5 | 426 | COV_P[1][1]=pre_COV_P[1][1]; |
Joeatsumi | 0:f41e4813b4c5 | 427 | |
Joeatsumi | 0:f41e4813b4c5 | 428 | } |
Joeatsumi | 0:f41e4813b4c5 | 429 | |
Joeatsumi | 0:f41e4813b4c5 | 430 | pc.printf("%f,%f,Roll=%f,Pitch=%f\r\n",threshold_acc,threshold_acc_ini,MAT_X_k[0][0],MAT_X_k[1][0]); |
Joeatsumi | 0:f41e4813b4c5 | 431 | //pc.printf("2;Roll=%f,Pitch=%f\r\n",COV_P[0][0],COV_P[1][1]); |
Joeatsumi | 0:f41e4813b4c5 | 432 | |
Joeatsumi | 0:f41e4813b4c5 | 433 | /*--------------------------------------------------------------------*/ |
Joeatsumi | 0:f41e4813b4c5 | 434 | |
Joeatsumi | 0:f41e4813b4c5 | 435 | return; |
Joeatsumi | 0:f41e4813b4c5 | 436 | |
Joeatsumi | 0:f41e4813b4c5 | 437 | } |
Joeatsumi | 0:f41e4813b4c5 | 438 | |
Joeatsumi | 0:f41e4813b4c5 | 439 | |
Joeatsumi | 0:f41e4813b4c5 | 440 | //================================================== |
Joeatsumi | 0:f41e4813b4c5 | 441 | //Main |
Joeatsumi | 0:f41e4813b4c5 | 442 | //================================================== |
Joeatsumi | 0:f41e4813b4c5 | 443 | int main() { |
Joeatsumi | 0:f41e4813b4c5 | 444 | |
Joeatsumi | 0:f41e4813b4c5 | 445 | //UART initialization |
Joeatsumi | 0:f41e4813b4c5 | 446 | pc.baud(115200); |
Joeatsumi | 0:f41e4813b4c5 | 447 | |
Joeatsumi | 0:f41e4813b4c5 | 448 | //gyro and accelerometer initialization |
Joeatsumi | 0:f41e4813b4c5 | 449 | offset_calculation_for_gyro(); |
Joeatsumi | 0:f41e4813b4c5 | 450 | |
Joeatsumi | 0:f41e4813b4c5 | 451 | //identify initilal atitude |
Joeatsumi | 0:f41e4813b4c5 | 452 | aquisition_sensor_values(acc,gyro); |
Joeatsumi | 0:f41e4813b4c5 | 453 | atitude_estimation_from_accelerometer(acc,MAT_X_k[0]); |
Joeatsumi | 0:f41e4813b4c5 | 454 | |
Joeatsumi | 0:f41e4813b4c5 | 455 | threshold_acc_ini=sqrt(acc[0]*acc[0]+acc[1]*acc[1]+acc[2]*acc[2]); |
Joeatsumi | 0:f41e4813b4c5 | 456 | |
Joeatsumi | 0:f41e4813b4c5 | 457 | /*Initialize MAT_A*/ |
Joeatsumi | 0:f41e4813b4c5 | 458 | MAT_A[0][0]=1.0; |
Joeatsumi | 0:f41e4813b4c5 | 459 | MAT_A[0][1]=0.0; |
Joeatsumi | 0:f41e4813b4c5 | 460 | MAT_A[1][0]=0.0; |
Joeatsumi | 0:f41e4813b4c5 | 461 | MAT_A[1][1]=1.0; |
Joeatsumi | 0:f41e4813b4c5 | 462 | |
Joeatsumi | 0:f41e4813b4c5 | 463 | mat_tran(MAT_A[0], inv_MAT_A[0], 2, 2); |
Joeatsumi | 0:f41e4813b4c5 | 464 | /*----------------*/ |
Joeatsumi | 0:f41e4813b4c5 | 465 | |
Joeatsumi | 0:f41e4813b4c5 | 466 | /*Initialize MAT_B*/ |
Joeatsumi | 0:f41e4813b4c5 | 467 | MAT_B[0][0]=1.0/FRQ; |
Joeatsumi | 0:f41e4813b4c5 | 468 | MAT_B[0][1]=0.0; |
Joeatsumi | 0:f41e4813b4c5 | 469 | MAT_B[1][0]=0.0; |
Joeatsumi | 0:f41e4813b4c5 | 470 | MAT_B[1][1]=1.0/FRQ; |
Joeatsumi | 0:f41e4813b4c5 | 471 | |
Joeatsumi | 0:f41e4813b4c5 | 472 | mat_tran(MAT_B[0], inv_MAT_B[0], 2, 2); |
Joeatsumi | 0:f41e4813b4c5 | 473 | /*----------------*/ |
Joeatsumi | 0:f41e4813b4c5 | 474 | |
Joeatsumi | 0:f41e4813b4c5 | 475 | /*Initialize MAT_C*/ |
Joeatsumi | 0:f41e4813b4c5 | 476 | MAT_C[0][0]=1.0; |
Joeatsumi | 0:f41e4813b4c5 | 477 | MAT_C[0][1]=0.0; |
Joeatsumi | 0:f41e4813b4c5 | 478 | MAT_C[1][0]=0.0; |
Joeatsumi | 0:f41e4813b4c5 | 479 | MAT_C[1][1]=1.0; |
Joeatsumi | 0:f41e4813b4c5 | 480 | |
Joeatsumi | 0:f41e4813b4c5 | 481 | mat_tran(MAT_C[0], inv_MAT_C[0], 2, 2); |
Joeatsumi | 0:f41e4813b4c5 | 482 | /*----------------*/ |
Joeatsumi | 0:f41e4813b4c5 | 483 | |
Joeatsumi | 0:f41e4813b4c5 | 484 | /* |
Joeatsumi | 0:f41e4813b4c5 | 485 | これら共分散の値は仮の値である。 |
Joeatsumi | 0:f41e4813b4c5 | 486 | */ |
Joeatsumi | 0:f41e4813b4c5 | 487 | /*Initialize COV_P*/ |
Joeatsumi | 0:f41e4813b4c5 | 488 | COV_P[0][0]=1.0; |
Joeatsumi | 0:f41e4813b4c5 | 489 | COV_P[0][1]=0.0; |
Joeatsumi | 0:f41e4813b4c5 | 490 | COV_P[1][0]=0.0; |
Joeatsumi | 0:f41e4813b4c5 | 491 | COV_P[1][1]=1.0; |
Joeatsumi | 0:f41e4813b4c5 | 492 | /*----------------*/ |
Joeatsumi | 0:f41e4813b4c5 | 493 | |
Joeatsumi | 0:f41e4813b4c5 | 494 | /*Initialize COV_Q*/ |
Joeatsumi | 0:f41e4813b4c5 | 495 | COV_Q[0][0]=0.01; |
Joeatsumi | 0:f41e4813b4c5 | 496 | COV_Q[0][1]=0.0; |
Joeatsumi | 0:f41e4813b4c5 | 497 | COV_Q[1][0]=0.0; |
Joeatsumi | 0:f41e4813b4c5 | 498 | COV_Q[1][1]=0.01; |
Joeatsumi | 0:f41e4813b4c5 | 499 | /*----------------*/ |
Joeatsumi | 0:f41e4813b4c5 | 500 | |
Joeatsumi | 0:f41e4813b4c5 | 501 | /*Initialize COV_R*/ |
Joeatsumi | 0:f41e4813b4c5 | 502 | COV_R[0][0]=0.001; |
Joeatsumi | 0:f41e4813b4c5 | 503 | COV_R[0][1]=0.0; |
Joeatsumi | 0:f41e4813b4c5 | 504 | COV_R[1][0]=0.0; |
Joeatsumi | 0:f41e4813b4c5 | 505 | COV_R[1][1]=0.001; |
Joeatsumi | 0:f41e4813b4c5 | 506 | /*----------------*/ |
Joeatsumi | 0:f41e4813b4c5 | 507 | |
Joeatsumi | 0:f41e4813b4c5 | 508 | pc.printf("Roll=%f,Pitch=%f\r\n",COV_P[0][0],COV_P[1][1]); |
Joeatsumi | 0:f41e4813b4c5 | 509 | |
Joeatsumi | 0:f41e4813b4c5 | 510 | //Ticker |
Joeatsumi | 0:f41e4813b4c5 | 511 | flipper.attach(&atitude_update, 0.01); |
Joeatsumi | 0:f41e4813b4c5 | 512 | |
Joeatsumi | 0:f41e4813b4c5 | 513 | while(1) { |
Joeatsumi | 0:f41e4813b4c5 | 514 | wait(0.01); |
Joeatsumi | 0:f41e4813b4c5 | 515 | |
Joeatsumi | 0:f41e4813b4c5 | 516 | } |
Joeatsumi | 0:f41e4813b4c5 | 517 | } |