Kalman filter problem Development for MPU6050

[kemzansdinars]

I developed a sumo robot code that needs to plan and move from a set location to the center of the ring. To do this, a mathematical model has been created and using the MPU6050 sensor, combining them with the Kalman filter, the exact position of the robot is obtained x={x;y;theta;x';y';theta'}. I have also observed from the serial monitor that the K matrix does not have 6 values ​​(so that there is one for each x state), and the existing K values ​​are not in the interval from 0 to 1. Maybe someone can help me find where I went wrong?

This is the developed code, where read_imu is intended to obtain x, y and z angle data. In turn, update_kalman_v2 is used for the Kalman filter, as well as move_to_origin which performs the movement (but the program has been changed so that the robot cannot move at this moment). Below I have attached the data from the serial monitor.

#include "Arduino.h"
#include "robot.h"

//#define DEBUG_MODE
#define DEBUG_MODE_PK

robot::robot() {
    robotGood = true;
    A = 0.078;
    dt = 0.01;
    V_left = 0;
    V_right = 0;
    theta_icc = 0;
    target_reached = false; 
}

void print3x3(const char* name, float M[3][3]){
  Serial.println(name);
  for(int i = 0; i < 3; i++){
    for(int j = 0; j < 3; j++){
      Serial.print(M[i][j], 3);
      Serial.print("  ");
    }
    Serial.println();
  }
  Serial.println();
}
void print6x6(const char* name, float M[6][6]){
  Serial.println(name);
  for(int i = 0; i < 6; i++){
    for(int j = 0; j < 6; j++){
      Serial.print(M[i][j], 3);
      Serial.print("  ");
    }
    Serial.println();
  }
  Serial.println();
}

void prin3x6(const char* name, float M[3][6]) {
  Serial.println(name);
  for(int i = 0; i < 3; i++){
    for(int j = 0; j < 6; j++){
      Serial.print(M[i][j], 3);
      Serial.print("  ");
    }
    Serial.println();
  }
  Serial.println();
}
void prin6x3(const char* name, float M[6][3]) {
  Serial.println(name);
  for(int i = 0; i < 3; i++){
    for(int j = 0; j < 3; j++){
      Serial.print(M[i][j], 3);
      Serial.print("  ");
    }
    Serial.println();
  }
  Serial.println();
}

void print1x6(const char* name, float M[6]) {
  Serial.println(name);
    for(int j = 0; j < 6; j++){
      Serial.print(M[j], 3);
      Serial.print("  ");
  }
  Serial.println();
}

void robot::begin() {
    Wire.begin();
    if (!mpu.begin()) {  // Adafruit_MPU6050
        Serial.println("MPU6050 con good");
        while (1);
    }
    mpu.setAccelerometerRange(MPU6050_RANGE_2_G);
    mpu.setGyroRange(MPU6050_RANGE_250_DEG); 

    mpu.setFilterBandwidth(MPU6050_BAND_94_HZ); 

    pinMode(pwm_left, OUTPUT);
    pinMode(dir_left, OUTPUT);
    pinMode(pwm_right, OUTPUT);
    pinMode(dir_right, OUTPUT);
    pinMode(sen_LEFT_pin, INPUT);
    pinMode(sen_RIGHT_pin, INPUT);
    pinMode(sen_BACK_pin, INPUT);
//    pinMode(sharp_1_pin, INPUT);
    pinMode(sharp_2_pin, INPUT);
    pinMode(sharp_3_pin, INPUT);
    pinMode(sharp_4_pin, INPUT);
//    pinMode(sharp_5_pin, INPUT);


    float F_init[6][6] = {
        {1, 0, 0, dt, 0, 0},
        {0, 1, 0, 0, dt, 0},
        {0, 0, 1, 0, 0, dt},
        {0, 0, 0, 1, 0, 0},
        {0, 0, 0, 0, 1, 0},
        {0, 0, 0, 0, 0, 1}
    };
    float H_init[3][6] = {
        {0, 0, 0, 1, 0, 0},
        {0, 0, 0, 0, 1, 0},
        {0, 0, 0, 0, 0, 1}
    };
    float Q_init[6][6] = {
        {0.01, 0, 0, 0, 0, 0},
        {0, 0.01, 0, 0, 0, 0},
        {0, 0, 0.01, 0, 0, 0},
        {0, 0, 0, 0.1, 0, 0},
        {0, 0, 0, 0, 0.1, 0},
        {0, 0, 0, 0, 0, 0.1}
    };
    float R_init[3][3] = {
        {0.01, 0, 0},
        {0, 0.01, 0},
        {0, 0, 0.01}
    };
    memcpy(F, F_init, sizeof(F));
    memcpy(H, H_init, sizeof(H));
    memcpy(Q, Q_init, sizeof(Q));
    memcpy(R, R_init, sizeof(R));
    for (int i = 0; i < 6; i++) {
        x[i] = 0;
        for (int j = 0; j < 6; j++) {
            P[i][j] = (i == j) ? 1.0 : 0;
        }
    }
    calibrate_mpu();

    #ifdef DEBUG_MODE_PK
      print3x3("R", R);
      print6x6("F_init", F_init);
    #endif
}

void robot::set_initial_state(float x0, float y0, float theta0) {
    x[0] = x0;
    x[1] = y0;
    x[2] = theta0;
    x[3] = 0;
    x[4] = 0;
    x[5] = 0;
    theta_icc = theta0;
}

float ax_offset = 0, ay_offset = 0, az_offset = 0, gz_offset = 0;
float ax_var = 0.01, ay_var = 0.01, gz_var = 0.01;

void robot::calibrate_mpu() {
    const int samples = 1000;
    float ax_sum = 0, ay_sum = 0, az_sum = 0, gz_sum = 0;
    float ax_var_sum = 0, ay_var_sum = 0, gz_var_sum = 0;

    Serial.println("Calibration proccessssss");
    for (int i = 0; i < samples; i++) {
        sensors_event_t a, g, temp;
        mpu.getEvent(&a, &g, &temp);
        ax_sum += a.acceleration.x;
        ay_sum += a.acceleration.y;
        az_sum += a.acceleration.z;
        gz_sum += g.gyro.z;
        delay(1);
    }
    ax_offset = ax_sum / samples;
    ay_offset = ay_sum / samples;
    az_offset = az_sum / samples;
    gz_offset = gz_sum / samples;

    for (int i = 0; i < samples; i++) {
        sensors_event_t a, g, temp;
        mpu.getEvent(&a, &g, &temp);
        ax_var_sum += pow(a.acceleration.x - ax_offset, 2);
        ay_var_sum += pow(a.acceleration.y - ay_offset, 2);
        gz_var_sum += pow(g.gyro.z - gz_offset, 2);
        delay(1);
    }
    ax_var = ax_var_sum / (samples - 1);
    ay_var = ay_var_sum / (samples - 1);
    gz_var = gz_var_sum / (samples - 1);

//    float R_init[3][3] = {
//        {ax_var, 0, 0},
//        {0, ay_var, 0},
//        {0, 0, gz_var}
//    };
//    memcpy(R, R_init, sizeof(R));
//    #ifdef DEBUG_MODE_PK
//      print3x3("R_init_ in calibration", R_init);
//    #endif
}

void robot::read_imu() {
    static float last_ax = 0, last_ay = 0;
    static unsigned long last_time = 0;
    sensors_event_t a, g, temp;
    mpu.getEvent(&a, &g, &temp);
    unsigned long current_time = millis();
    float dt_imu = (current_time - last_time) / 1000.0;
    last_time = current_time;

    float ax = a.acceleration.x - ax_offset;
    float ay = a.acceleration.y - ay_offset;
    float gz = g.gyro.z - gz_offset;

    
    static float ax_filt = 0, ay_filt = 0, gz_filt = 0;
//    float alpha = 0.5;
//    ax_filt = alpha * ax_filt + (1 - alpha) * ax;
//    ay_filt = alpha * ay_filt + (1 - alpha) * ay;
//    gz_filt = alpha * gz_filt + (1 - alpha) * gz;
    ax_filt = ax;
    ay_filt = ay;
    gz_filt = gz;
    
    z[0] = last_ax + ax_filt * dt_imu;
    z[1] = last_ay + ay_filt * dt_imu;
    z[2] = gz_filt;

    last_ax = z[0];
    last_ay = z[1];
//    Serial.print(z[0]);
//    Serial.print("   ");
//    Serial.print(z[1]);
//    Serial.print("   ");
//    Serial.print(z[2]);
//    Serial.println("   ");
    
}

void robot::move_to_origin() {
    if (target_reached) {
          SetStop();
          return;
    }
    float distance = sqrt(x[0] * x[0] + x[1] * x[1]);
    if (distance < 0.1) { // 10 cm tolerance
        Serial.println("Target reached");
        SetStop();
        target_reached = true;
        return;
    }
    float target_angle = atan2(-x[1], -x[0]);
    float angle_error = target_angle - x[2];
    angle_error = atan2(sin(angle_error), cos(angle_error));
    float speed = 150;
    float turn_speed = 100 * angle_error;
    V_left = speed - turn_speed;
    V_right = speed + turn_speed;
    SetSpeed(V_left, V_right);
    SetFront();
    Serial.print("Pos: x=");
    Serial.print(x[0]);
    Serial.print(", y=");
    Serial.print(x[1]);
    Serial.print(", theta=");
    Serial.println(x[2]);
}

void robot::_add_motor_pins(int in1PIN, int in2PIN, int in3PIN, int in4PIN) {
    pwm_left = in1PIN;
    dir_left = in2PIN;
    pwm_right = in3PIN;
    dir_right = in4PIN;
}

void robot::_add_lineSensor_pins(int senLeft, int senRight, int senBack) {
    sen_LEFT_pin = senLeft;
    sen_RIGHT_pin = senRight;
    sen_BACK_pin = senBack;
}

void robot::_add_sharpSensors_pins(int in1, int in2, int in3, int in4, int in5) {
//    sharp_1_pin = in1;
    sharp_2_pin = in2;
    sharp_3_pin = in3;
    sharp_4_pin = in4;
//    sharp_5_pin = in5;
}

int robot::sensor_state_cal() {
    int sensors_mot_State = 0;
    SHARP2 = !digitalRead(sharp_2_pin);
    SHARP3 = !digitalRead(sharp_3_pin);
    SHARP4 = !digitalRead(sharp_4_pin);
    senRight = analogRead(sen_RIGHT_pin) < 50;
    senBack = analogRead(sen_BACK_pin) < 50;
    senLeft = analogRead(sen_LEFT_pin) < 50;

    if (senLeft) sensors_mot_State += 1;
    if (senRight) sensors_mot_State += 2;
    if (senBack) sensors_mot_State += 4;
    if (sensors_mot_State == 0) {
        if (SHARP2) sensors_mot_State += 8;
        if (SHARP3) sensors_mot_State += 16;
        if (SHARP4) sensors_mot_State += 32;
    }
    return sensors_mot_State;
}

void robot::SetBack() {
//    digitalWrite(dir_left, HIGH); digitalWrite(dir_right, HIGH);
}

void robot::SetFront() {
//    digitalWrite(dir_left, LOW); digitalWrite(dir_right, LOW);
}

void robot::SetLeft() {
//    digitalWrite(dir_left, HIGH); digitalWrite(dir_right, LOW);
}

void robot::SetRight() {
//    digitalWrite(dir_left, LOW); digitalWrite(dir_right, HIGH);
}

void robot::SetStop() {
//    analogWrite(pwm_left, 0); analogWrite(pwm_right, 0);
    V_left = 0;
    V_right = 0;
}

void robot::SetSpeed(int _speedA, int _speedB) {
    speed_left = constrain(_speedA, 0, 255);
    speed_right = constrain(_speedB, 0, 255);
    V_left = speed_left / 255.0 * 0.5; // conver to m/s
    V_right = speed_right / 255.0 * 0.5;
//    analogWrite(pwm_left, speed_left);
//    analogWrite(pwm_right, speed_right);
}

void robot::update_kalman_v2() {
    static unsigned long last_time = 0.01;
    unsigned long current_time = millis();
    dt = (current_time - last_time) / 1000.0;
    last_time = current_time;
    F[0][3] = dt; F[1][4] = dt; F[2][5] = dt;
    
    // prediction step
    // U are control matrix
    B[0][0] = 0; B[0][1] = 0;
    B[1][0] = 0; B[1][1] = 0;
    B[2][0] = 0; B[2][1] = 0;
    B[3][0] = cos(theta_icc) / 2; B[3][1] = cos(theta_icc) / 2;
    B[4][0] = sin(theta_icc) / 2; B[4][1] = sin(theta_icc) / 2;
    B[5][0] = -1 / A; B[5][1] = 1 / A;

    #ifdef DEBUG_MODE
      Serial.println("B 2x6");
      Serial.println("0 0\n0 0\n0 0");
      Serial.print(B[3][0]);
      Serial.println(B[3][1]);
      Serial.print(B[4][0]);
      Serial.println(B[4][1]);
      Serial.print(B[5][0]);
      Serial.println(B[5][1]);
    #endif

    V_left = 0;
    V_right = 0;
    float u[2] = {V_left, V_right};
    #ifdef DEBUG_MODE
      Serial.println("u 1x2");
      Serial.print(V_left);
      Serial.println(V_right);
    #endif
    
    float x_pred[6];
    for (int i = 0; i < 6; i++) {
        x_pred[i] = 0;
        for (int j = 0; j < 6; j++) {
            x_pred[i] += F[i][j] * x[j];
        }
        for (int j = 0; j < 2; j++) {
            x_pred[i] += B[i][j] * u[j];
        }
    }
    #ifdef DEBUG_MODE
      print1x6("x_pred[1x6]", x_pred);
    #endif
    float temp[6][6];
    for (int i=0; i<6; i++){
        for (int j=0; j<6; j++){
            temp[i][j] = 0;
            for (int k=0; k<6; k++){
                temp[i][j] += F[i][k] * P[k][j];
            }
        }
    }
    float P_pred[6][6];
    for (int i=0; i<6; i++){
        for (int j=0; j<6; j++){
            P_pred[i][j] = Q[i][j];
            for (int k=0; k<6; k++){
                P_pred[i][j] += temp[i][k] * F[j][k];  // F^ transponēšana
            }
        }
    }
    #ifdef DEBUG_MODE_PK
      print6x6("P_pred [6x6]", P_pred);
    #endif

    // AUpdate step
    Serial.println("Red");
    read_imu();
    float Hx[3];
    for (int i = 0; i < 3; i++) {
        Hx[i] = 0;
        for (int j = 0; j < 6; j++) {
            Hx[i] += H[i][j] * x_pred[j]; // [3x6] * [1x6]
        }
    }
    float HP[3][6];
    for (int i = 0; i < 3; i++) {
        for (int j = 0; j < 6; j++) {
            HP[i][j] = 0;
            for (int k = 0; k < 6; k++) {
                HP[i][j] += H[i][k] * P_pred[k][j];
            }
        }
    }

    float PH_T[6][3];
    for (int i = 0; i < 6; i++) {
        for (int j = 0; j < 3; j++) {
            PH_T[i][j] = 0;
            for (int k = 0; k < 6; k++) {
                PH_T[i][j] += P_pred[i][k] * H[j][k]; // H^T
            }
        }
    }
    // S = HP * H^T + R
    float S[3][3];
    for (int i = 0; i < 3; i++) {
        for (int j = 0; j < 3; j++) {
            S[i][j] = R[i][j];
            for (int k = 0; k < 6; k++) {
                for (int l = 0; l < 6; l++) {
                    S[i][j] += H[i][k] * P_pred[k][l] * H[j][l];
                }
            }
        }
    }
    #ifdef DEBUG_MODE_PK
      print3x3("S[3][3]", S);
    #endif


    // LU decomposition and inversion
    float L[3][3] = {{1, 0, 0}, {0, 1, 0}, {0, 0, 1}};
    float U[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}};
    float S_copy[3][3];
    memcpy(S_copy, S, sizeof(float) * 9);
    for (int i = 0; i < 3; i++) {
        U[i][i] = S_copy[i][i];
        for (int k = i + 1; k < 3; k++) {
            if (U[i][i] != 0) {
                L[k][i] = S_copy[k][i] / U[i][i];
            }
            U[i][k] = S_copy[i][k];
        }
        for (int j = i + 1; j < 3; j++) {
            for (int k = i + 1; k < 3; k++) {
                S_copy[j][k] -= L[j][i] * U[i][k];
            }
        }
    }
    
    float S_inv[3][3];
    for (int j = 0; j < 3; j++) {
        float y[3] = {0, 0, 0};
        y[j] = 1;
        for (int i = 1; i < 3; i++) {
            for (int k = 0; k < i; k++) {
                y[i] -= L[i][k] * y[k];
            }
        }
        float x[3];
        for (int i = 2; i >= 0; i--) {
            x[i] = y[i];
            for (int k = i + 1; k < 3; k++) {
                x[i] -= U[i][k] * x[k];
            }
            if (fabs(U[i][i]) > 1e-6) {
                x[i] /= U[i][i];
            } else {
                x[i] = 0;
            }
        }
        for (int i = 0; i < 3; i++) {
            S_inv[i][j] = x[i];
        }
    }
    #ifdef DEBUG_MODE_PK
      print3x3("S_inv 3x3", S_inv);
    #endif
    // Kalman gain and update
    float K[6][3];
    for (int i = 0; i < 6; i++) {
        for (int j = 0; j < 3; j++) {
            K[i][j] = 0;
            for (int k = 0; k < 3; k++) {
                K[i][j] += PH_T[i][k] * S_inv[k][j];
            }
        }
    }

    #ifdef DEBUG_MODE_PK
      prin6x3("K [6x3]", K);
    #endif
    for (int i = 0; i < 6; i++) {
        x[i] = x_pred[i];
        for (int j = 0; j < 3; j++) {
            x[i] += K[i][j] * (z[j] - Hx[j]);
        }
    }
    #ifdef DEBUG_MODE_PK
      print1x6("new 1x6", x);
    #endif
    for (int i = 0; i < 6; i++) {
        for (int j = 0; j < 6; j++) {
            float temp = 0;
            for (int k = 0; k < 3; k++) {
                temp += K[i][k] * H[k][j];
            }
            P[i][j] = P_pred[i][j] - temp;
        }
    }
    #ifdef DEBUG_MODE_PK
      print6x6("new P", P);
    #endif
    theta_icc = x[2];
}

R
0.010  0.000  0.000  
0.000  0.010  0.000  
0.000  0.000  0.010  

F_init
1.000  0.000  0.000  0.010  0.000  0.000  
0.000  1.000  0.000  0.000  0.010  0.000  
0.000  0.000  1.000  0.000  0.000  0.010  
0.000  0.000  0.000  1.000  0.000  0.000  
0.000  0.000  0.000  0.000  1.000  0.000  
0.000  0.000  0.000  0.000  0.000  1.000  

P_pred [6x6]
133.283  0.000  0.000  11.501  0.000  0.000  
0.000  133.283  0.000  0.000  11.501  0.000  
0.000  0.000  133.283  0.000  0.000  11.501  
11.501  0.000  0.000  1.100  0.000  0.000  
0.000  11.501  0.000  0.000  1.100  0.000  
0.000  0.000  11.501  0.000  0.000  1.100  

Red
S[3][3]
1.110  0.000  0.000  
0.000  1.110  0.000  
0.000  0.000  1.110  

S_inv 3x3
0.901  0.000  0.000  
0.000  0.901  0.000  
0.000  0.000  0.901  

K [6x3]
10.361  0.000  0.000  
0.000  10.361  0.000  
0.000  0.000  10.361  

new 1x6
5.010  0.377  1.573  0.517  0.036  0.000  
new P
133.283  0.000  0.000  1.140  0.000  0.000  
0.000  133.283  0.000  0.000  1.140  0.000  
0.000  0.000  133.283  0.000  0.000  1.140  
11.501  0.000  0.000  0.109  0.000  0.000  
0.000  11.501  0.000  0.000  0.109  0.000  
0.000  0.000  11.501  0.000  0.000  0.109  

Pos: x=5.01, y=0.38, theta=1.57

next
P_pred [6x6]
146.956  0.000  0.000  1.256  0.000  0.000  
0.000  146.956  0.000  0.000  1.256  0.000  
0.000  0.000  146.956  0.000  0.000  1.256  
11.618  0.000  0.000  0.209  0.000  0.000  
0.000  11.618  0.000  0.000  0.209  0.000  
0.000  0.000  11.618  0.000  0.000  0.209  

Red
S[3][3]
0.219  0.000  0.000  
0.000  0.219  0.000  
0.000  0.000  0.219  

S_inv 3x3
4.566  0.000  0.000  
0.000  4.566  0.000  
0.000  0.000  4.566  

K [6x3]
5.737  0.000  0.000  
0.000  5.737  0.000  
0.000  0.000  5.737  

new 1x6
5.502  0.415  1.573  0.507  0.036  0.000  
new P
146.956  0.000  0.000  -4.481  0.000  0.000  
0.000  146.956  0.000  0.000  -4.481  0.000  
0.000  0.000  146.956  0.000  0.000  -4.481  
11.618  0.000  0.000  -0.745  0.000  0.000  
0.000  11.618  0.000  0.000  -0.745  0.000  
0.000  0.000  11.618  0.000  0.000  -0.745  

Pos: x=5.50, y=0.41, theta=1.57

next
P_pred [6x6]
153.755  0.000  0.000  -5.279  0.000  0.000  
0.000  153.755  0.000  0.000  -5.279  0.000  
0.000  0.000  153.755  0.000  0.000  -5.279  
10.819  0.000  0.000  -0.645  0.000  0.000  
0.000  10.819  0.000  0.000  -0.645  0.000  
0.000  0.000  10.819  0.000  0.000  -0.645  

Red
S[3][3]
-0.635  0.000  0.000  
0.000  -0.635  0.000  
0.000  0.000  -0.635  

S_inv 3x3
-1.574  0.000  0.000  
0.000  -1.574  0.000  
0.000  0.000  -1.574  

K [6x3]
8.309  0.000  0.000  
0.000  8.309  0.000  
0.000  0.000  8.309  

new 1x6
6.114  0.364  1.569  0.515  0.025  -0.000  
new P
153.755  0.000  0.000  -13.588  0.000  0.000  
0.000  153.755  0.000  0.000  -13.588  0.000  
0.000  0.000  153.755  0.000  0.000  -13.588  
10.819  0.000  0.000  -1.661  0.000  0.000  
0.000  10.819  0.000  0.000  -1.661  0.000  
0.000  0.000  10.819  0.000  0.000  -1.661  

Pos: x=6.11, y=0.36, theta=1.57

next
P_pred [6x6]
148.895  0.000  0.000  -15.367  0.000  0.000  
0.000  148.895  0.000  0.000  -15.367  0.000  
0.000  0.000  148.895  0.000  0.000  -15.367  
9.040  0.000  0.000  -1.561  0.000  0.000  
0.000  9.040  0.000  0.000  -1.561  0.000  
0.000  0.000  9.040  0.000  0.000  -1.561  

Red
S[3][3]
-1.551  0.000  0.000  
0.000  -1.551  0.000  
0.000  0.000  -1.551  

S_inv 3x3
-0.645  0.000  0.000  
0.000  -0.645  0.000  
0.000  0.000  -0.645  

K [6x3]
9.907  0.000  0.000  
0.000  9.907  0.000  
0.000  0.000  9.907  

new 1x6
6.446  0.198  1.570  0.493  0.005  -0.000  
new P
148.895  0.000  0.000  -25.274  0.000  0.000  
0.000  148.895  0.000  0.000  -25.274  0.000  
0.000  0.000  148.895  0.000  0.000  -25.274  
9.040  0.000  0.000  -2.568  0.000  0.000  
0.000  9.040  0.000  0.000  -2.568  0.000  
0.000  0.000  9.040  0.000  0.000  -2.568  

Pos: x=6.45, y=0.20, theta=1.57

next
P_pred [6x6]
128.574  0.000  0.000  -28.024  0.000  0.000  
0.000  128.574  0.000  0.000  -28.024  0.000  
0.000  0.000  128.574  0.000  0.000  -28.024  
6.291  0.000  0.000  -2.468  0.000  0.000  
0.000  6.291  0.000  0.000  -2.468  0.000  
0.000  0.000  6.291  0.000  0.000  -2.468  

Red
S[3][3]
-2.458  0.000  0.000  
0.000  -2.458  0.000  
0.000  0.000  -2.458  

S_inv 3x3
-0.407  0.000  0.000  
0.000  -0.407  0.000  
0.000  0.000  -0.407  

K [6x3]
11.403  0.000  0.000  
0.000  11.403  0.000  
0.000  0.000  11.403  

new 1x6
7.076  0.243  1.583  0.502  0.009  0.001  
new P
128.574  0.000  0.000  -39.427  0.000  0.000  
0.000  128.574  0.000  0.000  -39.427  0.000  
0.000  0.000  128.574  0.000  0.000  -39.427  
6.291  0.000  0.000  -3.472  0.000  0.000  
0.000  6.291  0.000  0.000  -3.472  0.000  
0.000  0.000  6.291  0.000  0.000  -3.472  

Pos: x=7.08, y=0.24, theta=1.58

next
P_pred [6x6]
89.031  0.000  0.000  -43.152  0.000  0.000  
0.000  89.031  0.000  0.000  -43.152  0.000  
0.000  0.000  89.031  0.000  0.000  -43.152  
2.566  0.000  0.000  -3.372  0.000  0.000  
0.000  2.566  0.000  0.000  -3.372  0.000  
0.000  0.000  2.566  0.000  0.000  -3.372  

Red
S[3][3]
-3.362  0.000  0.000  
0.000  -3.362  0.000  
0.000  0.000  -3.362  

S_inv 3x3
-0.297  0.000  0.000  
0.000  -0.297  0.000  
0.000  0.000  -0.297  

K [6x3]
12.837  0.000  0.000  
0.000  12.837  0.000  
0.000  0.000  12.837  

new 1x6
7.826  0.559  1.579  0.519  0.033  0.001  
new P
89.031  0.000  0.000  -55.989  0.000  0.000  
0.000  89.031  0.000  0.000  -55.989  0.000  
0.000  0.000  89.031  0.000  0.000  -55.989  
2.566  0.000  0.000  -4.375  0.000  0.000  
0.000  2.566  0.000  0.000  -4.375  0.000  
0.000  0.000  2.566  0.000  0.000  -4.375  

Pos: x=7.83, y=0.56, theta=1.58

next
P_pred [6x6]
26.807  0.000  0.000  -60.674  0.000  0.000  
0.000  26.807  0.000  0.000  -60.674  0.000  
0.000  0.000  26.807  0.000  0.000  -60.674  
-2.119  0.000  0.000  -4.275  0.000  0.000  
0.000  -2.119  0.000  0.000  -4.275  0.000  
0.000  0.000  -2.119  0.000  0.000  -4.275  

Red
S[3][3]
-4.265  0.000  0.000  
0.000  -4.265  0.000  
0.000  0.000  -4.265  

S_inv 3x3
-0.234  0.000  0.000  
0.000  -0.234  0.000  
0.000  0.000  -0.234  

K [6x3]
14.228  0.000  0.000  
0.000  14.228  0.000  
0.000  0.000  14.228  

new 1x6
8.979  0.532  1.578  0.561  0.028  0.000  
new P
26.807  0.000  0.000  -74.902  0.000  0.000  
0.000  26.807  0.000  0.000  -74.902  0.000  
0.000  0.000  26.807  0.000  0.000  -74.902  
-2.119  0.000  0.000  -5.277  0.000  0.000  
0.000  -2.119  0.000  0.000  -5.277  0.000  
0.000  0.000  -2.119  0.000  0.000  -5.277  

Pos: x=8.98, y=0.53, theta=1.58

[jremington]

@kemzansdinars

the exact position of the robot is obtained

In theory, but not in practice with the MPU6050, or any consumer grade IMU. They are too noisy and inaccurate, among other problems.

A module that could do that would be very handy, and if it were just a matter of code, you would be able to buy it on eBay very cheaply.

[kemzansdinars]

Okay @jremington, thanks for the answer. But as you can see in the serial output, the theta angle remains constant when the robot is not moving. So there is no angle error, but the x position and y position change. So I'm looking for a way to keep these two parameters constant when the robot is not moving. This raises suspicions that the data from the MPU6050 sensor is not qualitative, but the Kalman gain in the Kalman filter function is not in the range from 0 to 1.

[jremington]

Yes, the code may have serious errors, in addition to the fact that it won't give useful results even if correct.

You don't need real data to test proper code function, so, if you are looking for a learning experience, debug the code and algorithm with synthetic input data, for cases where you know the correct outcome.

By the way, it is not completely clear what your system model is. If you want help from the forum, add some useful comments.