※なにか気になる点がありましたらコメント欄にご記入ください。
※この記事は以下のリンクから貼られており、倒立振子という創作物に関するコードの一部を載せています。詳しくはリンク元をご覧ください。
倒立振子に挑む - Tanuki_Bayashin’s diary
使用しているマイコンは Raspberry Pi Pico と呼ばれるものです。
記述はC言語を用いています。（pico－SDK C/C++ という環境を使用しています）
ここではカルマンフィルタに関する処理を載せています。（大元はトラ技2019年７月号の特集に依っています）
筆者はあまりカルマンフィルタについて理解していないので、詳しく説明することはできないです。
統計学的な手法により、センサーなどから得られた信号から、ノイズを除去することができるのだそうです。
ディジタルフィルタとの違いは、位相の遅れを気にしなくていいくらいにしか理解していません。悪しからず。
リスト４内の処理：
theta_estimation()
倒立振子の本体の傾斜角と、傾斜角の測定値のオフセットを推定しています。main() 関数内にて2.5[ms] おきにタイマー割り込みにより、処理を呼び出しています。
x_estimation()
倒立振子の４つの状態変数である本体の傾斜角と車輪の回転角、およびそれらの時間微分（角速度）を推定しています。
リスト４ カルマンフィルタに関する処理の部分
//========================================================= //Kalman filter for "theta" & "theta_dot_bias" //It takes 410 usec. (Raspberry Pi Pico 125MHz, MPU6050) //========================================================= bool theta_estimation(struct repeating_timer* t) { // (2022/12/01) //detach the rotary encoder polling //cancelled_encorder = cancel_repeating_timer(&timer_encorder); //t_start = time_us_32(); float y2; float theta_dot_gyro; float P_CT[2][1] = {}; float tran_C_theta[2][1] = {}; float G_temp1[1][1] = {}; float G_temp2; float G[2][1] = {}; float C_theta_theta[1][1] = {}; float delta_y; float delta_theta[2][1] = {}; float GC[2][2] = {}; float I2[2][2] = { {1,0},{0,1} }; float I2_GC[2][2] = {}; float A_theta_theta[2][1] = {}; float B_theta_dot[2][1] = {}; float AP[2][2] = {}; float APAT[2][2] = {}; float tran_A_theta[2][2] = {}; float BBT[2][2]; float tran_B_theta[1][2] = {}; float BUBT[2][2] = {}; //measurement data y2 = get_P_angle(); //degree theta_dot_gyro = get_P_angle_dot(); //degree/sec //calculate Kalman gain: G = P'C^T(W+CP'C^T)^-1 mat_tran(C_theta[0], tran_C_theta[0], 1, 2);//C^T mat_mul(P_theta_predict[0], tran_C_theta[0], P_CT[0], 2, 2, 2, 1);//P'C^T mat_mul(C_theta[0], P_CT[0], G_temp1[0], 1, 2, 2, 1);//CP'C^T G_temp2 = 1.0f / (G_temp1[0][0] + theta_variance);//(W+CP'C^T)^-1 mat_mul_const(P_CT[0], G_temp2, G[0], 2, 1);//P'C^T(W+CP'C^T)^-1 //theta_data estimation: theta = theta'+G(y-Ctheta') mat_mul(C_theta[0], theta_data_predict[0], C_theta_theta[0], 1, 2, 2, 1);//Ctheta' delta_y = y2 - C_theta_theta[0][0];//y-Ctheta' mat_mul_const(G[0], delta_y, delta_theta[0], 2, 1); mat_add(theta_data_predict[0], delta_theta[0], theta_data[0], 2, 1); //calculate covariance matrix: P=(I-GC)P' mat_mul(G[0], C_theta[0], GC[0], 2, 1, 1, 2);//GC mat_sub(I2[0], GC[0], I2_GC[0], 2, 2);//I-GC mat_mul(I2_GC[0], P_theta_predict[0], P_theta[0], 2, 2, 2, 2);//(I-GC)P' //predict the next step data: theta'=Atheta+Bu mat_mul(A_theta[0], theta_data[0], A_theta_theta[0], 2, 2, 2, 1);//Atheta mat_mul_const(B_theta[0], theta_dot_gyro, B_theta_dot[0], 2, 1);//Bu mat_add(A_theta_theta[0], B_theta_dot[0], theta_data_predict[0], 2, 1);//Atheta+Bu //predict covariance matrix: P'=APA^T + BUB^T mat_tran(A_theta[0], tran_A_theta[0], 2, 2);//A^T mat_mul(A_theta[0], P_theta[0], AP[0], 2, 2, 2, 2);//AP mat_mul(AP[0], tran_A_theta[0], APAT[0], 2, 2, 2, 2);//APA^T mat_tran(B_theta[0], tran_B_theta[0], 2, 1);//B^T mat_mul(B_theta[0], tran_B_theta[0], BBT[0], 2, 1, 1, 2);//BB^T mat_mul_const(BBT[0], theta_dot_variance, BUBT[0], 2, 2);//BUB^T mat_add(APAT[0], BUBT[0], P_theta_predict[0], 2, 2);//APA^T+BUB^T // (2022/12/01) //attach a timer for the rotary encoder (40 kHz) //add_repeating_timer_us(-25, rotary_encoder_check, NULL, &timer_encorder); //sleep_us(10); //count_esti++; //t_end = time_us_32() - t_start; return true; } void x_estimation() { //variables for Kalman gain calculation float theta1_dot_temp; float tran_C_x[4][4]; float P_CT4[4][4]; float G_temp14[4][4]; float G_temp24[4][4]; float G_temp24_inv[4][4]; float G4[4][4]; //variables for x_hat estimation float C_x_x[4][1]; float delta_y4[4][1]; float delta_x[4][1]; //variables for covariance matrix calculation float GC4[4][4]; float I4[4][4] = { {1, 0, 0, 0}, {0, 1, 0, 0}, {0, 0, 1, 0}, {0, 0, 0, 1} }; float I4_GC[4][4]; //variables for x prediction float Vin; float A_x_x[4][1]; float B_x_Vin[4][1]; //variables for covariance prediction float tran_A_x[4][4]; float AP4[4][4]; float AP4AT[4][4]; float BBT4[4][4]; float tran_B_x[1][4]; float BUBT4[4][4]; //measurement data //y_input[0][0] = theta_data[0][0] * 3.14f / 180.0f; y_input[0][0] = get_P_angle() * 3.14f / 180.0f; //radian theta1_dot_temp = get_P_angle_dot(); y_input[1][0] = (theta1_dot_temp - theta_data[1][0]) * 3.14f / 180.0f; y_input[2][0] = encoder_value / 1074.0 * 3.141593f * 2.0; y_input[3][0] = (y_input[2][0] - pre_theta2) / feedback_rate; //y_input[2][0] = W_angle[0] * 3.14f / 180.0f; // calculate_W_angle_dot(); // y[3][0] = W_angle_dot * 3.14f / 180; //calculate Kalman gain: G = P'C^T(W+CP'C^T)^-1 mat_tran(C_x[0], tran_C_x[0], 4, 4);//C^T mat_mul(P_x_predict[0], tran_C_x[0], P_CT4[0], 4, 4, 4, 4);//P'C^T mat_mul(C_x[0], P_CT4[0], G_temp14[0], 4, 4, 4, 4);//CPC^T mat_add(G_temp14[0], measure_variance_mat[0], G_temp24[0], 4, 4);//W+CP'C^T mat_inv(G_temp24[0], G_temp24_inv[0], 4, 4);//(W+CP'C^T)^-1 mat_mul(P_CT4[0], G_temp24_inv[0], G4[0], 4, 4, 4, 4); //P'C^T(W+CP'C^T)^-1 //x_data estimation: x = x'+G(y-Cx') mat_mul(C_x[0], x_data_predict[0], C_x_x[0], 4, 4, 4, 1);//Cx' mat_sub(y_input[0], C_x_x[0], delta_y4[0], 4, 1);//y-Cx' mat_mul(G4[0], delta_y4[0], delta_x[0], 4, 4, 4, 1);//G(y-Cx') mat_add(x_data_predict[0], delta_x[0], x_data[0], 4, 1);//x'+G(y-Cx') //calculate covariance matrix: P=(I-GC)P' mat_mul(G4[0], C_x[0], GC4[0], 4, 4, 4, 4);//GC mat_sub(I4[0], GC4[0], I4_GC[0], 4, 4);//I-GC mat_mul(I4_GC[0], P_x_predict[0], P_x[0], 4, 4, 4, 4);//(I-GC)P' //predict the next step data: x'=Ax+Bu Vin = volts; if (volts > 3.3f) { Vin = 3.3f; } if (volts < -3.3f) { Vin = -3.3f; } mat_mul(A_x[0], x_data[0], A_x_x[0], 4, 4, 4, 1);//Ax_hat mat_mul_const(B_x[0], Vin, B_x_Vin[0], 4, 1);//Bu mat_add(A_x_x[0], B_x_Vin[0], x_data_predict[0], 4, 1);//Ax+Bu //predict covariance matrix: P'=APA^T + BUB^T mat_tran(A_x[0], tran_A_x[0], 4, 4);//A^T mat_mul(A_x[0], P_x[0], AP4[0], 4, 4, 4, 4);//AP mat_mul(AP4[0], tran_A_x[0], AP4AT[0], 4, 4, 4, 4);//APA^T mat_tran(B_x[0], tran_B_x[0], 4, 1);//B^T mat_mul(B_x[0], tran_B_x[0], BBT4[0], 4, 1, 1, 4);//BB^T mat_mul_const(BBT4[0], voltage_variance, BUBT4[0], 4, 4);//BUB^T mat_add(AP4AT[0], BUBT4[0], P_x_predict[0], 4, 4);//APA^T+BUB^T return; }
