Atsumi Toda / Mbed 2 deprecated Auto_pilot_atitude_estimaion_KF

ドローン用計測制御基板の作り方 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?

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