※なにか気になる点がありましたらコメント欄にご記入ください。また、工作や回路を製作する場合には、細かい作業などに対して、細心の注意を払われるようお願いいたします。
【目次】
１．はじめに
前々からこのブログにて傾斜計の試作を重ねてきましたが、ある程度形になったので、以下に紹介したいと思います。
傾斜計自体の出典は以下に示すトランジスタ技術のバックナンバーによります。詳しい内容については、こちらをご参照いただければと思います。
出典：トランジスタ技術 ２０１９年７月号 第１章第２節（P.３９～）のあたり
toragi.cqpub.co.jp
また以下は、以前Pythonにてコードを書いた時の傾斜計の試作機についての記事です。傾斜角を求める原理や、傾斜計の回路についても載せてあります。参考にしてください。
tanuki-bayashin.hatenablog.com
さらに今回Raspberry Pi Pico を用いたことから、Pico-SDKのC/C++環境に載っていたサンプルプログラムを利用しました。トランジスタ技術のソースコードを利用した分も含めて、この場を借りて感謝の意を表します。ありがとうございました。
※あくまでここに載せてある内容は、電子工作レベルの作品です。実験的に試作し、動作の確認を楽しむレベルであることをご承知おきください。
２．ソースコード
２．１ 全体の構造
図１にソースコードの全体の流れを示します。加速度センサーから値を読み取り、適切な処理を行うことにより傾斜角を求めています（カルマンフィルターを用いています）。最後にUARTにてPC側にデータを送信しています。
（このコードでは、出典の付録に載っていたソースコードをもとにしています。所々、ソースコードのままの部分もあります。ご承知おきください）
※確かにカルマンフィルターを用いているのですが、自分はカルマンフィルターの詳しい理論については何も知識はないです。
C言語にて記述していますが、使用しているMPUがラズパイPicoということで、加速度センサー（MPU6050）、UART、GPIＯ、タイマー割り込みなどの処理はPico-SDKのサンプルプログラムやネット上の情報を利用させていただきました。
２．２ ソースコード
リスト１にソースコードを示します。601行あります。大まかではありますが、２．３節にて解説を載せています。
（Cmakeを使う場合に、ファイルを複数に分けたときの記述の仕方が分からなかったため、1つのｃファイルのままにしました。申し訳ないです）
リスト１ 傾斜計内部のソースコード
//========================================================= //Inclinometer //MPU board: Raspberry Pi Pico //Accelerometer + Gyro sensor: MPU6050 //2022/08/06 @Tanuki_Bayashin //========================================================= #include <stdio.h> #include <string.h> #include <math.h> #include <stdlib.h> #include "pico/stdlib.h" #include "hardware/uart.h" #include "pico/binary_info.h" #include "hardware/gpio.h" #include "hardware/i2c.h" //========================================================= //Port Setting //DigitalIN BUTTON //Tactile Switch //DigitalOut LED_RED, LED_BLUE // LEDs //I2C SDA:GP4, SCL: GP5(MPU6050) //Gyro + Accelerometer (SDA, SCL) //Serial UART: Uart1, Tx: GP8, Rx: GP9 // UART (TX, RX) //========================================================= #define UART_ID uart1 #define BAUD_RATE 115200 #define UART_TX_PIN 8 #define UART_RX_PIN 9 #define BUTTON 0 #define LED_RED 1 #define LED_BLUE 15 #define PI 3.1415926535898 #define ONE_G 16383 // 2^14 - 1 = 16383 for gravity compensation // address 0x68 for I2C static int addr = 0x68; // to use MPU6050 // for timer interrupt volatile bool timer_fired = false; // true:LED ON, false:LED OFF bool LED_flag = false; //========================================================= //Accelerometer and gyro statistical data //========================================================= int16_t acceleration[3], gyro[3]; int16_t* temperature; int sample_num = 1000; float meas_interval = 0.001; // 1.0 / sample_num float theta_mean; float theta_variance; float theta_dot_mean; float theta_dot_variance; float angle, angle_dot; //========================================================= //Accelerometer and gyro offset //========================================================= // variables for offset of raw accelaration data float acc_Y_offset = 0.0; float acc_Z_offset = 0.0; // for offset of angular velocity float gyro_X_offset = 0.0; //========================================================= //Kalman filter (for angle estimation) variables //========================================================= //Update rate int theta_update_freq = 400; //Hz float theta_update_interval = 0.0025; // (1.0 / (float)theta_update_freq) //State vector //[[theta(degree)], [offset of theta_dot(degree/sec)]] //float theta_data_predict[2][1]; float theta_data_predict[2][1] = { {1},{1} }; float theta_data[2][1]; //Covariance matrix float P_theta_predict[2][2]; float P_theta[2][2]; //"A" of the state equation float A_theta[2][2] = { {1, -0.0025}, {0, 1} }; // 0.0025: theta_update_interval //"B" of the state equation float B_theta[2][1] = { {0.0025}, {0} }; // 0.0025: theta_update_interval //"C" of the state equation float C_theta[1][2] = { {1, 0} }; //========================================================= // Matrix common functions //========================================================= //Matrix addition void mat_add(float* m1, float* m2, float* sol, int row, int column) { for (int i = 0; i < row; i++) { for (int j = 0; j < column; j++) { sol[i * column + j] = m1[i * column + j] + m2[i * column + j]; } } return; } //Matrix subtraction void mat_sub(float* m1, float* m2, float* sol, int row, int column) { for (int i = 0; i < row; i++) { for (int j = 0; j < column; j++) { sol[i * column + j] = m1[i * column + j] - m2[i * column + j]; } } return; } //Matrix multiplication void mat_mul(float* m1, float* m2, float* sol, int row1, int column1, int row2, int column2) { for (int i = 0; i < row1; i++) { for (int j = 0; j < column2; j++) { sol[i * column2 + j] = 0; for (int k = 0; k < column1; k++) { sol[i * column2 + j] += m1[i * column1 + k] * m2[k * column2 + j]; } } } return; } //Matrix transposition void mat_tran(float* m1, float* sol, int row_original, int column_original) { for (int i = 0; i < row_original; i++) { for (int j = 0; j < column_original; j++) { sol[j * row_original + i] = m1[i * column_original + j]; } } return; } //Matrix scalar maltiplication void mat_mul_const(float* m1, float c, float* sol, int row, int column) { for (int i = 0; i < row; i++) { for (int j = 0; j < column; j++) { sol[i * column + j] = c * m1[i * column + j]; } } return; } //Matrix inversion (by Gaussian elimination) void mat_inv(float* m, float* sol, int column, int row) { //allocate memory for a temporary matrix float* temp = (float*)malloc(column * 2 * row * sizeof(float)); //make the augmented matrix for (int i = 0; i < column; i++) { //copy original matrix for (int j = 0; j < row; j++) { temp[i * (2 * row) + j] = m[i * row + j]; } //make identity matrix for (int j = row; j < row * 2; j++) { if (j - row == i) { temp[i * (2 * row) + j] = 1; } else { temp[i * (2 * row) + j] = 0; } } } //Sweep (down) for (int i = 0; i < column; i++) { //pivot selection float pivot = temp[i * (2 * row) + i]; int pivot_index = i; float pivot_temp; for (int j = i; j < column; j++) { if (temp[j * (2 * row) + i] > pivot) { pivot = temp[j * (2 * row) + i]; pivot_index = j; } } if (pivot_index != i) { for (int j = 0; j < 2 * row; j++) { pivot_temp = temp[pivot_index * (2 * row) + j]; temp[pivot_index * (2 * row) + j] = temp[i * (2 * row) + j]; temp[i * (2 * row) + j] = pivot_temp; } } //division for (int j = 0; j < 2 * row; j++) { temp[i * (2 * row) + j] /= pivot; } //sweep for (int j = i + 1; j < column; j++) { float temp2 = temp[j * (2 * row) + i]; //sweep each row for (int k = 0; k < row * 2; k++) { temp[j * (2 * row) + k] -= temp2 * temp[i * (2 * row) + k]; } } } //Sweep (up) for (int i = 0; i < column - 1; i++) { for (int j = i + 1; j < column; j++) { float pivot = temp[(column - 1 - j) * (2 * row) + (row - 1 - i)]; for (int k = 0; k < 2 * row; k++) { temp[(column - 1 - j) * (2 * row) + k] -= pivot * temp[(column - 1 - i) * (2 * row) + k]; } } } //copy result for (int i = 0; i < column; i++) { for (int j = 0; j < row; j++) { sol[i * row + j] = temp[i * (2 * row) + (j + row)]; } } free(temp); return; } //========================================================= // Accelerometer (MPU6050) // Gyroscope //========================================================= // int16_t acceleration[3], gyro[3]; // int16_t* temperature; // //get data //float theta_mean; //float theta_variance; //float theta_dot_mean; //float theta_dot_variance; #ifdef i2c_default static void mpu6050_reset() { // Two byte reset. First byte register, second byte data // There are a load more options to set up the device in different ways that could be added here uint8_t buf[] = { 0x6B, 0x00 }; i2c_write_blocking(i2c_default, addr, buf, 2, false); // set angle velocity ±500[degree/sec] buf[0] = 0x1B; buf[1] = 0x01; i2c_write_blocking(i2c_default, addr, buf, 2, false); // set accelaration ±2G buf[0] = 0x1C; buf[1] = 0x00; i2c_write_blocking(i2c_default, addr, buf, 2, false); // set LPF BW 260Hz(0.0ms delay) for Accel, // 256Hz(0.98ms) for Gyro buf[0] = 0x1A; buf[1] = 0x00; i2c_write_blocking(i2c_default, addr, buf, 2, false); } void get_angle() { uint8_t buffer[4]; // Start reading acceleration registers from register 0x3B for 6 bytes uint8_t val = 0x3D; i2c_write_blocking(i2c_default, addr, &val, 1, true); // true to keep master control of bus i2c_read_blocking(i2c_default, addr, buffer, 4, false); acceleration[1] = buffer[0] << 8 | buffer[1]; acceleration[2] = buffer[2] << 8 | buffer[3]; angle = atan2((double)acceleration[1] - acc_Y_offset, (double)acceleration[2] - acc_Z_offset) * 180.0 / PI; return; // [degree] } void get_gyro_data() { uint8_t buffer[2]; //float angle_dot; float number; // Now gyro data from reg 0x43 for 6 bytes uint8_t val = 0x43; i2c_write_blocking(i2c_default, addr, &val, 1, true); i2c_read_blocking(i2c_default, addr, buffer, 2, false); // False - finished with bus gyro[0] = buffer[0] << 8 | buffer[1]; // 2^15 - 1 = 32767, ±500 [degree/s] for Full range angle_dot = (gyro[0] - gyro_X_offset) * 500.0 / 32767.0; // [degree/s] return; } #endif void Calibration() { //// Calculate these variables // acc_Y_offset; // acc_Z_offset; // gyro_X_offset; // // theta_mean; // theta_variance; // theta_dot_mean; // theta_dot_variance; //float angle, angle_dot; float acc_Y_sum = 0.0; float acc_Z_sum = 0.0; float gyro_X_sum = 0.0; float angle_sum = 0.0; float angle_dot_sum = 0.0; float temp_std; float ONE_G_sin, ONE_G_cos; // #define ONE_G 16383 = 2^14 - 1 ONE_G_sin = ONE_G * sin(-30.0 * PI / 180.0); // あらかじめ重力分の計算をしておく ONE_G_cos = ONE_G * cos(-30.0 * PI / 180.0); /////////////////////////////////////////////// // Calculate offset of acc and gyro raw data // /////////////////////////////////////////////// for (int i = 0; i < sample_num; i++) { get_angle(); get_gyro_data(); acc_Y_sum += acceleration[1] - ONE_G_sin; acc_Z_sum += acceleration[2] - ONE_G_cos; gyro_X_sum += gyro[0]; // delay 1[ms] sleep_ms(1); } // results of calculation of mean values acc_Y_offset = acc_Y_sum / sample_num; acc_Z_offset = acc_Z_sum / sample_num; gyro_X_offset = gyro_X_sum / sample_num; /////////////////////////////////////// // Calculate mean value of variables // /////////////////////////////////////// angle_sum = 0.0; angle_dot_sum = 0.0; for (int i = 0; i < sample_num; i++) { get_angle(); get_gyro_data(); angle_sum += angle; angle_dot_sum += angle_dot; // delay 1[ms] sleep_ms(1); } theta_mean = angle_sum / sample_num; // [degree] theta_dot_mean = angle_dot_sum / sample_num; // [degree/s] /////////////////////////////////////////// // Calculate variance value of variables // /////////////////////////////////////////// angle_sum = 0.0; angle_dot_sum = 0.0; for (int i = 0; i < sample_num; i++) { get_angle(); get_gyro_data(); temp_std = angle - theta_mean; angle_sum += temp_std * temp_std; temp_std = angle_dot - theta_dot_mean; angle_dot_sum += temp_std * temp_std; // delay 1[ms] sleep_ms(1); } theta_variance = angle_sum / sample_num; // [degree]^2 theta_dot_variance = angle_dot_sum / sample_num; // [degree/s]^2 return; } //========================================================= //Kalman filter for "theta" & "theta_dot_bias" //It takes 650 usec. (NUCLEO-F401RE 84MHz, BMX055) ※Picoに移植する前の名残りです //========================================================= bool repeating_timer_callback(struct repeating_timer* t) { // カルマンフィルタの計算をしています（タイマー割り込み使用） //measurement data get_angle(); //degree float y = angle; //degree //input data get_gyro_data(); float theta_dot_gyro = angle_dot; //degree/sec //calculate Kalman gain: G = P'C^T(W+CP'C^T)^-1 float P_CT[2][1] = {}; float tran_C_theta[2][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 float G_temp1[1][1] = {}; mat_mul(C_theta[0], P_CT[0], G_temp1[0], 1, 2, 2, 1);//CP'C^T float G_temp2 = 1.0f / (G_temp1[0][0] + theta_variance);//(W+CP'C^T)^-1 float G[2][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') float C_theta_theta[1][1] = {}; mat_mul(C_theta[0], theta_data_predict[0], C_theta_theta[0], 1, 2, 2, 1);//Ctheta' float delta_y = y - C_theta_theta[0][0];//y-Ctheta' float delta_theta[2][1] = {}; 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' float GC[2][2] = {}; float I2[2][2] = { {1,0},{0,1} }; mat_mul(G[0], C_theta[0], GC[0], 2, 1, 1, 2);//GC float I2_GC[2][2] = {}; 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 float A_theta_theta[2][1] = {}; float B_theta_dot[2][1] = {}; 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 float AP[2][2] = {}; float APAT[2][2] = {}; float tran_A_theta[2][2] = {}; 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 float BBT[2][2]; float tran_B_theta[1][2] = {}; 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 float BUBT[2][2] = {}; 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 // LED_BLUE toggle (ON/OFF)（LEDの点滅です） if (LED_flag) { gpio_put(LED_BLUE, 1); } else { gpio_put(LED_BLUE, 0); } return true; } void main() { int count = 0; int count_for_LED = 0; int count_for_UART = 0; int start = 0; static char s[32]; stdio_init_all(); // 何かを初期化しているようです // Set up our UART with the required speed. uart_init(UART_ID, BAUD_RATE); // Set the TX and RX pins by using the function select on the GPIO // Set datasheet for more information on function select gpio_set_function(UART_TX_PIN, GPIO_FUNC_UART); gpio_set_function(UART_RX_PIN, GPIO_FUNC_UART); // Use some the various UART functions to send out data // In a default system, printf will also output via the default UART #if !defined(i2c_default) || !defined(PICO_DEFAULT_I2C_SDA_PIN) ||!defined(PICO_DEFAULT_I2C_SCL_PIN) #warning i2c / mpu6050_i2c example requires a board with I2C pins uart_puts(UART_ID, "Default I2C pins were not defined"); #else uart_puts(UART_ID, "Hello, MPU6050! Reading raw data from registers..."); // This example will use I2C0 on the default SDA and SCL pins (4, 5 on a Pico) i2c_init(i2c_default, 400 * 1000); gpio_set_function(PICO_DEFAULT_I2C_SDA_PIN, GPIO_FUNC_I2C); gpio_set_function(PICO_DEFAULT_I2C_SCL_PIN, GPIO_FUNC_I2C); gpio_pull_up(PICO_DEFAULT_I2C_SDA_PIN); gpio_pull_up(PICO_DEFAULT_I2C_SCL_PIN); // Make the I2C pins available to picotool bi_decl(bi_2pins_with_func(PICO_DEFAULT_I2C_SDA_PIN, PICO_DEFAULT_I2C_SCL_PIN, GPIO_FUNC_I2C)); mpu6050_reset(); #endif // 加速度、角速度のオフセットおよび、傾斜角とその時間微分の平均、分散を計算 Calibration(); sleep_ms(1); // GPIO(for LEDs and BUTTON) initialize gpio_init(LED_RED); gpio_init(LED_BLUE); gpio_set_dir(LED_RED, GPIO_OUT); gpio_set_dir(LED_BLUE, GPIO_OUT); gpio_init(BUTTON); gpio_set_dir(BUTTON, GPIO_IN); gpio_pull_up(BUTTON); struct repeating_timer timer; add_repeating_timer_ms(-2.5, repeating_timer_callback, NULL, &timer); sleep_ms(1); while (1) { // BUTTON ON -> LED_RED ON and vice vasa if (!gpio_get(BUTTON)) { gpio_put(LED_RED, 1); } else { gpio_put(LED_RED, 0); } if (count_for_LED >= 50) { LED_flag = !LED_flag; count_for_LED = 0; } count_for_LED++; // 実際にはangle とtheta_data[0][0]を別々に送信してます。 //（１つの角度に対して2回実験を行っています） // angle をuartにてPC側へ送信（PC側での瞬時値に当たります） snprintf(s, sizeof(s), "%f", angle); uart_puts(UART_ID, s); // theta_data[0][0] をuartにてPC側へ送信（PC側での推定値に当たります） //snprintf(s, sizeof(s), "%f", theta_data[0][0]); //uart_puts(UART_ID, s); uart_puts(UART_ID, "\n"); sleep_ms(10); } bool cancelled = cancel_repeating_timer(&timer); uart_puts(UART_ID, "cancelled... "); sendData_integer(cancelled); uart_puts(UART_ID, "\0"); return; }
２．３ ソースコードの解説
以下に大まかな解説を記述しました。
解説：
８－17行 必要なインクルード・ファイルを読み込んでいます。
19－44行 UARTやI2Cを使うときに必要なマクロの指定や、GPIO（LEDやswに使う）に関する指定などを行っています。
45－57行 傾斜角度を推定するのに使用する加速度や角速度、およびそれらの平均、分散などの変数の宣言です。
58－66行 加速度センサーからの生データに含まれるオフセット値用の変数です。
67ー90行 カルマンフィルターによる傾斜角の推定値を求める処理に使用する変数（ベクトルや行列含む）です。
91－260行 行列の計算をする関数群です。加算、減算、行列同士の積、転置、スカラー倍、逆行列の計算の処理を行う関数をまとめて記述しています。
261ー427行 加速度センサーを使うときに使用する関数群です。
275行ー mpu6050_reset() 初期設定を行います。
299行ー get_angle() 角度を取得します。
318行ー get_gyro_data() 角速度を取得します。
339行ー Calibration() 加速度の値と角速度の値のオフセット、角度と角速度それぞれの平均、分散を求めます。
429ー502行 カルマンフィルターによって傾斜角を計算している部分です。（このプログラムの心臓部です）
504ー608行 main関数です。
506行ー 変数の宣言です。
512行ー UART や加速度センサーを使用する前の初期設定をしています。
547行ー キャリブレーション、GPIOの設定、タイマー割り込みの設定です。
566行ー 無限ループです。LEDを点滅させたり、角度のデータをUARTにてPCに送信しています。
598行ー 後処理です。この前の処理が無限ループなので、ここが実行されることはまずないです。
３．測定結果
２章のソースコードをビルドし、ラズパイPicoによって動かしたところ、以下のような結果が得られました。
図３と図４は、傾斜角を一定にした状態で測定を行った場合の測定結果です。（図２は傾斜角０°、図３は傾斜角 ―３０°）それぞれ瞬時値とカルマンフィルターを通した場合に、データがどのような分布になるかをヒストグラムで表示したものです。
図を見てもある程度分かるかと思いますが、表１のようにいろいろな傾斜角のときの平均値と標準偏差を計算しました。
上の結果をグラフで表示したのが図５です。（グラフをクリックすると大きく表示されます）
傾斜角の瞬時値とカルマンフィルターを通した値とでは、表1の通り平均では瞬時値のほうが約３０倍よく、標準偏差の場合ではカルマンフィルタを通した値のほうが約３０倍いいという結果となりました。
グラフで見ると図５の通りです。平均値はいくらか推定値のほうが全体として約1°ほどズレがあります。また標準偏差を見てみると、推定値のほうが瞬時値のものよりも小さいようです。これはカルマンフィルターの働きにより推定値のばらつきが小さくなったとみていいと思われます。
倒立振子に適用したとき、どちらの値が適しているか比べるのも興味深いです。
４．まとめ
１章の出典をもとにカルマンフィルターを適用した傾斜計を作ってみました。
そして、カルマンフィルターにより傾斜角の推定値を求め、カルマンフィルタを通していない瞬時値と比較を行いました。その結果、平均では瞬時値のほうがよい値となりましたが、標準偏差ではカルマンフィルターを通した値のほうがよいということになりました。
今後の取り組みとしては、
- 現状のまま倒立振子を作り、動作が推定値と瞬時値でどう変わるか見てみる
- なにか改善点が思いつけば、取り組む
- カルマンフィルタにサーボ機構のようなものがつけられないか検討する（定常的な誤差を取り除きたい）
などが考えられます。（時間がかかりそうですが）
とりあえず今回は、結果が出たということで、ひと段落できた感じです。
付録 PC側のUART（シリアル通信）とデータ処理のプログラム
付録としてPC側のUART（シリアル通信）のプログラムを載せたいと思います。（Micro Python で記述しました）
処理の流れは、COMポートを開き、通信速度などの設定をした後、ラズパイPicoからのデータを取得します。そのあと、取得したデータをもとにヒストグラムを表示したり、平均と標準偏差を計算して処理は終わりになります。
（22-43行目のデータの取り扱いに関しては、他の方のブログを見て参考にさせて頂きました。よく理解できていないので、解説は控えます。ただ、data２の値をファイルに保存しています）
リスト２ PC側のUART（シリアル通信）のプログラム
import serial import numpy as np import matplotlib.pyplot as plt # str文字列がfloat()変換できるかどうかを判定する def is_float(s): try: float(s) except: return False return True COM = "COM13" bitRate = 115200 #受信用ポートをOpen ser = serial.Serial(COM, bitRate, timeout=0.1) f_real = open('data/angle_data_00.txt', 'w') data = [] data3 = [] count = 0 while count < 1000: a=ser.readline() moji2=a.decode('utf-8') moji3=moji2.strip() data=moji3.split(',') for term in data: if is_float(term): if ((float(term) < -180.0) or (float(term) > 180.0)): continue data2 = '{0:7.6f}'.format(float(term)) f_real.write(data2) f_real.write(',') data3.append(float(data2)) count += 1 ser.close() f_real.close() plt.hist( data3, bins=20, color="b") plt.show() mean = np.mean(data3) print('mean:', mean) std = np.std(data3) print('std:', std)
