nhatuan84 / arduino-kalmanfilter Goto Github PK

Hi nhatuan84

I am following your example to read data from an accelerometer (AccX, AccY, and AccZ) of three axes. It is an ICM20948 TDK 9-axes MEMS sensor. As it points out in one of the blogs you mentioned, the difficult part is to find good values for Q and R. I have followed your choice of 100.0f for R[0][0] and 0.01f for Q[0][0] but the estimated value is too slow to respond to the meausred value. The criteria is that the estimated curve should follow variation of noisy accelerometer data and it still can reject noise.

By various blind trials I have got the following setup which seems to work.

  m_kf.init(stateNum, measureNum, &A[0][0], &P[0][0], &Q[0][0], &H[0][0], &R[0][0], &xp[0], &xc[0]);
  m_kf.zeros();
  A[0][0] = 1.0f;
  H[0][0] = 1.0f;
  Q[0][0] = 0.2f;  //by blind trial
  R[0][0] = 50.0f; //by blind trial
  P[0][0] = 1.0f;

  xc[0] = read_register()/8.192; //Reading data from ICM20948 returns raw data from 16-bit register in milli-G with a resolution of 8192LSB/g = 8.192LSB/mg.

Screenshots below show the results :

My question is, besides blind trials like what I did, is there any way to find R and Q?

Thank you for sharing the code and library.

John