Kalman Filter Help!

Hello Arduino Community! It is my first post here so please excuse any Forum-related mistake I do.

I am currently working on an autonomous quadcopter project. I am using MultiWii as a flight controller and using an Arduino mega to control the MultiWii (via receiver inputs) according to some algorithms. The MultiWii and Arduino mega are communicating via Serial communication and I am able to receive attitude and position (from GPS) information from the MultiWii successfully. I am willing to implement a Kalman Filter to speed up the positioning refresh rate by combining both accelerometer and GPS data and predict x and y position along with x and y velocities only. I am totally familiar with Kalman predict and correct functions but however I would appreciate your help coding-wise.

I used and edited a code by Alex Hagiopol. Initially, I am verifying my global variables:

// For MultiWii-Arduino Communication
#include<MSP.h> 
// For Matrix Operations
#include<MatrixMath.h> 
#include<math.h>

/**************** KALMAN FILTER VARIABLES ***************/
float pi = 3.1415926;

unsigned long time;

int i = 1;
float cosLat = 0.0;
int64_t Rearth = 6378137;

float RcovarianceMatrix[2][2] = {
  {5.0, 13.51637078682249}, 
  {13.51637078682249, 5.0},
};
float QcovarianceMatrix[4][4] = {
  {0.01,    0,       0,      0    },
  {0,       0.01,    0,      0    },
  {0,       0,       0.01,  0    },
  {0,       0,       0,      0.01},
};
float Hmatrix[2][4] = {
  {1, 0, 0, 0},
  {0, 1, 0, 0},
};
float HmatrixTranspose[4][2] = {
  {1.0, 0.0},
  {0.0, 1.0},
  {0.0, 0.0},
  {0.0, 0.0},
};
float PerrorCovariance[4][4] = {
  {0.01, 0,     0,    0   },
  {0,     0.01, 0,    0   },
  {0,     0,     0.02, 0   },
  {0,     0,     0,    0.02},
};

float delta_T = 0; //((float) (GPS_data[i][2] - GPS_data[i-1][2])) / 1000; //Time elapsed in seconds
float data[2] = {0, 0}; //{(float) (GPS_data[i][0]-GPS_data[0][0])*pi/180*Rearth/10000000,(float) (GPS_data[i][1]-GPS_data[0][1])*pi/180*Rearth*cosLat/10000000};

float Xstate[4][1] = {
  {data[0]},
  {data[1]},
  {0.0    },
  {0.0    },
};
float accel_data[2][1] = {
  {0},
  {0},
};
float prevAccel_data[2][1] = {
  {0},
  {0},
};
int32_t GPS_data[1][3] = {
  {0, 0, 0},
};

int32_t prevGPS_data[1][3] = {
  {0, 0, 0},
};
int32_t firstGPS_data[1][3] = {
  {0, 0, 0},
};
/*************************************************************/

And then, The kalman_predict function (which works at the accelerometer refresh rate of 100 Hz) and the kalman_correct function (which works at the GPS refresh rate of 10 Hz).

float PerrorCovarianceEstimate[4][4] = {
  {0.0, 0.0, 0.0, 0.0},
  {0.0, 0.0, 0.0, 0.0},
  {0.0, 0.0, 0.0, 0.0},
  {0.0, 0.0, 0.0, 0.0},
};

float nextXstateEstimate[4][1] = {
  {0},
  {0},
  {0},
  {0},
};


void kalman_update()  {
  
  /*Kalman Update:
   X_k+1 = A.X_k + B.R.u
  P_k+1 = A.P_k.A^T
*/
  delta_T = ((float) (millis() - attitude_timer)) / 1000.0; //Time elapsed in seconds
  attitude_timer = millis();
  
  // State Evolution Matrix
  float Amatrix[4][4] = {
    {1, 0,  delta_T, 0      },
    {0, 1,  0,       delta_T},
    {0, 0,  1,       0      },
    {0, 0,  0,       1      },
  };
  
  // Control Matrix
  float Bmatrix[4][2] = {
    {0, 0},
    {0, 0},
    {delta_T, 0},
    {0, delta_T},
  };
  
  // R Matrix used to transform acceleration from body frame to inertial frame
  float Rmatrix[2][2] = {
    {cos(theta_rad)*cos(psi_rad), sin(phi_rad)*sin(theta_rad)*cos(psi_rad) - cos(phi_rad)*sin(psi_rad)},
    {cos(theta_rad)*sin(psi_rad), sin(phi_rad)*sin(theta_rad)*sin(psi_rad) + cos(phi_rad)*cos(psi_rad)}
  };
  float intermediateMatrix[4][1] = {
    {0},
    {0},
    {0},
    {0},
  };
  float intermediateMatrix2[4][2] = {
    {0, 0},
    {0, 0},
    {0, 0},
    {0, 0},
  };
  float intermediateMatrix3[4][1] = {
    {0},
    {0},
    {0},
    {0},
  };
  Matrix.Multiply((float*) Amatrix, (float*) Xstate, 4, 4, 1, (float*) intermediateMatrix);
  Matrix.Multiply((float*) Bmatrix, (float*) Rmatrix, 4, 2, 2, (float*) intermediateMatrix2);
  Matrix.Multiply((float*) intermediateMatrix2, (float*) prevAccel_data, 4, 2, 1, (float*) intermediateMatrix3);
  Matrix.Add((float*) intermediateMatrix3, (float*) intermediateMatrix, 4, 1, (float*) nextXstateEstimate);

  float AmatrixTranspose[4][4] = {
    {0.0, 0.0, 0.0, 0.0},
    {0.0, 0.0, 0.0, 0.0},
    {0.0, 0.0, 0.0, 0.0},
    {0.0, 0.0, 0.0, 0.0},
  };
  float IntermediateProductMatrix[4][4] = {
    {0.0, 0.0, 0.0, 0.0},
    {0.0, 0.0, 0.0, 0.0},
    {0.0, 0.0, 0.0, 0.0},
    {0.0, 0.0, 0.0, 0.0},
  };
  float IntermediateSumMatrix[4][4] = {
    {0.0, 0.0, 0.0, 0.0},
    {0.0, 0.0, 0.0, 0.0},
    {0.0, 0.0, 0.0, 0.0},
    {0.0, 0.0, 0.0, 0.0},
  };
  Matrix.Transpose((float*) Amatrix, 4 , 4, (float *) AmatrixTranspose);
  Matrix.Multiply((float*) Amatrix, (float*) PerrorCovariance, 4, 4, 4, (float*) IntermediateProductMatrix);
  Matrix.Multiply((float*) IntermediateProductMatrix, (float*) AmatrixTranspose, 4, 4, 4, (float*) IntermediateSumMatrix);
  Matrix.Add((float*) IntermediateSumMatrix, (float*) QcovarianceMatrix, 4, 4, (float*) PerrorCovarianceEstimate);
}


void kalman_correct() {
/*Kalman Correct:
   K_k = P_k+1.H^T.(H.P_k.H^T + R)^-1
  X_k = X_k+1 + K_k.(Z_k - H.X_k+1)
  P_k = (I - K_k.H).P_k+1
*/
  float KalmanGain[4][2] = {
    {0.0, 0.0},
    {0.0, 0.0},
    {0.0, 0.0},
    {0.0, 0.0},
  };
  float IntermediateProductMatrix2[2][4] = {
    {0.0, 0.0, 0.0, 0.0},
    {0.0, 0.0, 0.0, 0.0},
  };
  float IntermediateProductMatrix3[2][2] = {
    {0.0, 0.0},
    {0.0, 0.0},
  };
  float IntermediateQuotientMatrix[4][2] = {
    {0.0, 0.0},
    {0.0, 0.0},
    {0.0, 0.0},
    {0.0, 0.0},
  };
  float IntermediateSumMatrix2[2][2] = {
    {0.0, 0.0},
    {0.0, 0.0},
  };
  float IntermediateProductMatrix[4][4] = {
    {0.0, 0.0, 0.0, 0.0},
    {0.0, 0.0, 0.0, 0.0},
    {0.0, 0.0, 0.0, 0.0},
    {0.0, 0.0, 0.0, 0.0},
  };
  Matrix.Multiply((float*) Hmatrix, (float*) PerrorCovarianceEstimate, 2, 4, 4, (float*) IntermediateProductMatrix2);
  Matrix.Multiply((float*) IntermediateProductMatrix2, (float*) HmatrixTranspose, 2, 4, 2, (float*) IntermediateProductMatrix3);
  Matrix.Add((float*) IntermediateProductMatrix3, (float*) RcovarianceMatrix, 2, 2, (float*) IntermediateSumMatrix2);
  int flag = Matrix.Invert((float*) IntermediateSumMatrix2, 2); //The argument matrix gets inverted; no need to create temp matrix.
  if (flag == 0) {
    Serial.println("CANT INVERT");
  }
  Matrix.Multiply((float*) PerrorCovarianceEstimate, (float*) HmatrixTranspose, 4, 4, 2, (float*) IntermediateQuotientMatrix);
  Matrix.Multiply((float*) IntermediateQuotientMatrix, (float*) IntermediateSumMatrix2, 4, 2, 2, (float*) KalmanGain);

  data[0] = (float) (GPS_data[0][0] - firstGPS_data[0][0]) * pi / 180 * Rearth / 10000000;
  data[1] = (float) (GPS_data[0][1] - firstGPS_data[0][1]) * pi / 180 * Rearth * cosLat / 10000000;

  float ZkTranspose[2][1] = { //We only need the transposed version of this, so we do it right here.
    {data[0]},
    {data[1]},
  };
  float IntermediateProductMatrix4[2][1] = {
    {0.0},
    {0.0},
  };
  float IntermediateProductMatrix5[4][1] = {
    {0.0},
    {0.0},
    {0.0},
    {0.0},
  };
  float IntermediateSubtractionMatrix[2][1] = {
    {0.0},
    {0.0},
  };
  Matrix.Multiply((float*) Hmatrix, (float*) nextXstateEstimate, 2, 4, 1, (float*) IntermediateProductMatrix4);
  Matrix.Subtract((float*) ZkTranspose, (float*) IntermediateProductMatrix4, 2, 1, (float*) IntermediateSubtractionMatrix);
  Matrix.Multiply((float*) KalmanGain, (float*) IntermediateSubtractionMatrix, 4, 2, 1, (float*) IntermediateProductMatrix5); //Reuse intermediate matrix because it has appropriate dimensions
  Matrix.Add((float*) nextXstateEstimate, (float*) IntermediateProductMatrix5, 4, 1, (float*) Xstate); //NO NEED TO TRANSPOSE X STATE. WE DID THAT IN MATLAB FOR CONVENIENCE

  Matrix.Multiply((float*) KalmanGain, (float*) Hmatrix, 4, 2, 4, (float*) IntermediateProductMatrix); //Reuse this intermediate matrix because it's 4x4 and we need 4x4
  float IdentityMatrix[4][4] = {
    {1, 0, 0, 0},
    {0, 1, 0, 0},
    {0, 0, 1, 0},
    {0, 0, 0, 1},
  };
  Matrix.Subtract((float*) IdentityMatrix, (float*) IntermediateProductMatrix, 4, 4, (float*) IntermediateQuotientMatrix); //Reuse this intermediate matrix because it's 4x4 and we need 4x4.
  Matrix.Multiply((float*) IntermediateQuotientMatrix, (float*) PerrorCovarianceEstimate, 4, 4, 4, (float*) PerrorCovariance);

  i = i + 1;
}

And from my main loop:

// WILL RUN ON 100 HZ
  raw_attitude_counter++;
  if (raw_attitude_counter == 8) {
    raw_attitude_counter = 0;
    
    if (msp.request(MSP_RAW_IMU, &imu, sizeof(imu))) {

      prevAccel_data[0][0] = accel_data[0][0];
      prevAccel_data[1][0] = accel_data[1][0];
      float ax = imu.acc[1] / 255.00 * 9.8156f ;
      float ay = imu.acc[0] / 255.00 * 9.8156f ;
      // Filter Accelerometer Data by a Complimentary Filter
      accel_data[0][0] = 0.8 * accel_data[0][0] + 0.2 * ax;
      accel_data[1][0] = 0.8 * accel_data[1][0] + 0.2 * ay;

      if (accel_data[0][0] > -0.05 && accel_data[0][0] < 0.05) accel_data[0][0] = 0.0;
      if (accel_data[1][0] > -0.05 && accel_data[1][0] < 0.05) accel_data[1][0] = 0.0;

      kalman_update();
    }
  }

// Get GPS Updates Every 100 ms
  gps_counter++;
  if (gps_counter == 90) {
    gps_counter = 0;

    if (msp.request(MSP_RAW_GPS, &gps, sizeof(gps)))  {
      time = millis();
      prevGPS_data[0][0] = GPS_data[0][0];
      prevGPS_data[0][1] = GPS_data[0][1];
      prevGPS_data[0][2] = GPS_data[0][2];
      GPS_data[0][0] = gps.lat;
      GPS_data[0][1] = gps.lon;
      GPS_data[0][2] = (uint64_t) time;

      fixType = gps.fixType;
      numSat = gps.numSat;
      hdop = gps.hdop;
      fixType = gps.fixType;

      // Check For Good Connectivity
      if (numSat > 3) {
        if (i == 1) { //Initialize cosLat
          firstGPS_data[0][0] = GPS_data[0][0];
          firstGPS_data[0][1] = GPS_data[0][1];
          firstGPS_data[0][2] = GPS_data[0][2];
          cosLat = cos((float) firstGPS_data[0][0] / 10000000 * pi / 180);
        }
        kalman_correct();
      }
    }
  }

At first, I tried not fusing the accelerometer data at all (predict and correct steps are both working on 10 Hz as the GPS refresh rate) and got very good results but with slow update rate. But when fusing the accelerometer, the prediction step does not run at all even if I apply acceleration upon the quadcopter. When a correction value (from the GPS) is acquired, the prediction step starts accumulating errors massively (knowing that I am correcting the accelerometer biases initially making sure it is 0 m/s^2 when on a flat surface).

Sorry for writing this long but does anyone knows what am I doing wrong? (Kalman Equations are commented in the code)

That’s an advanced project.
Have checked the simple matter of the time required to get data, from GPS and from the accelerometer? I have an udea thae accelerometers might take some time to deliver data.
I guess… Test and read both data but don’t use the accelerometer data. Any change?

Thank you for your reply!

The accelerometer and GPS deliver data at exactly 100 Hz (dT = 10 ms) and 10 Hz (dT = 100 ms) respectively (I tested it!). When I remove the control input u (accelerometer input), I get very good filtration results but with low update rate (10 Hz).

So the GPS delivers data 10 times faster than the accelerometer.
I’ve not tried to analyze the code. It’s too much at this hour.
Generally it looks like reading the GPS 10 times and the accelerometer 1 time would keep the execution at the highest speed. Can You use such a data mix in the math and proceed?

Actually it is the inverse of this, the accelerometer delivers data 10 times faster than the GPS, unfortunately …

Sorry, I swapped facts. Nevertheless, know when fresh data is avalable from the 2 units. Don’t let the slow GPS mess up things if You can do usefull work out of the data from the accelerometer.
Use pen and paper, mark where data is avalable on the time line, and design the strategy from that.
The sun will soon raise and I need to go to sleep…
…

1 Like

KalmanFIlter is not going to do predictions.

This SImpleKalmanFilter works pretty good for filtering data.

loat RcovarianceMatrix[2][2] = {
  {5.0, 13.51637078682249}, 
  {13.51637078682249, 5.0},
};
float QcovarianceMatrix[4][4] = {
  {0.01,    0,       0,      0    },
  {0,       0.01,    0,      0    },
  {0,       0,       0.01,  0    },
  {0,       0,       0,      0.01},
};
float Hmatrix[2][4] = {
  {1, 0, 0, 0},
  {0, 1, 0, 0},
};
float HmatrixTranspose[4][2] = {
  {1.0, 0.0},
  {0.0, 1.0},
  {0.0, 0.0},
  {0.0, 0.0},
};
float PerrorCovariance[4][4] = {
  {0.01, 0,     0,    0   },
  {0,     0.01, 0,    0   },
  {0,     0,     0.02, 0   },
  {0,     0,     0,    0.02},
};


float Xstate[4][1] = {
  {data[0]},
  {data[1]},
  {0.0    },
  {0.0    },
};

loat PerrorCovarianceEstimate[4][4] = {
  {0.0, 0.0, 0.0, 0.0},
  {0.0, 0.0, 0.0, 0.0},
  {0.0, 0.0, 0.0, 0.0},
  {0.0, 0.0, 0.0, 0.0},
};

float nextXstateEstimate[4][1] = {
  {0},
  {0},
  {0},
  {0},
};

That is clearly a LinearRegression structure model. There are several fine LinearRegression Libraries. One library uses uint32_t the rest use uint64_t.

I used LinearRegression on a ESP32 to predict air pressure readings for the next hour with .98 correlation. Worked great on a ESP32, works even better on a Raspberry Pi.

1 Like

@Idahowalker
Thanks for your reply…

Actually, my problem is not with filtration, the filtration is going very well without an accelerometer… But in order to speed up the position refresh rate, I have to do Kalman prediction and here where the massive noises are added :woozy_face:

I will have a look on the SimpleKalmanFilter library!! Thanks a lot!!

In order for anyone to make sense of the code, you need to very clearly define the system model. What is that model?

I recommend to study this article by Dan Simon, which describes a couple of simple example systems. He first clearly explains the system models and then shows how the code is formulated.

1 Like

This topic was automatically closed 120 days after the last reply. New replies are no longer allowed.