state space of Crazyflie 2.0

[AHMAD SHARIFA]

I am a PhD student doing my research on Crazyflie, I want to ask about the Matrices A,B,C,D i.e. space state variables where its it written. I have seen the following part in kalman filter.c file is that correct ??
// ====== DYNAMICS LINEARIZATION ======
// Initialize as the identity
A[STATE_X][STATE_X] = 1;
A[STATE_Y][STATE_Y] = 1;
A[STATE_Z][STATE_Z] = 1;

A[STATE_PX][STATE_PX] = 1;
A[STATE_PY][STATE_PY] = 1;
A[STATE_PZ][STATE_PZ] = 1;

A[STATE_D0][STATE_D0] = 1;
A[STATE_D1][STATE_D1] = 1;
A[STATE_D2][STATE_D2] = 1;

// position from body-frame velocity
A[STATE_X][STATE_PX] = R[0][0]*dt;
A[STATE_Y][STATE_PX] = R[1][0]*dt;
A[STATE_Z][STATE_PX] = R[2][0]*dt;

A[STATE_X][STATE_PY] = R[0][1]*dt;
A[STATE_Y][STATE_PY] = R[1][1]*dt;
A[STATE_Z][STATE_PY] = R[2][1]*dt;

A[STATE_X][STATE_PZ] = R[0][2]*dt;
A[STATE_Y][STATE_PZ] = R[1][2]*dt;
A[STATE_Z][STATE_PZ] = R[2][2]*dt;

// position from attitude error
A[STATE_X][STATE_D0] = (S[STATE_PY]*R[0][2] - S[STATE_PZ]*R[0][1])*dt;
A[STATE_Y][STATE_D0] = (S[STATE_PY]*R[1][2] - S[STATE_PZ]*R[1][1])*dt;
A[STATE_Z][STATE_D0] = (S[STATE_PY]*R[2][2] - S[STATE_PZ]*R[2][1])*dt;

A[STATE_X][STATE_D1] = (- S[STATE_PX]*R[0][2] + S[STATE_PZ]*R[0][0])*dt;
A[STATE_Y][STATE_D1] = (- S[STATE_PX]*R[1][2] + S[STATE_PZ]*R[1][0])*dt;
A[STATE_Z][STATE_D1] = (- S[STATE_PX]*R[2][2] + S[STATE_PZ]*R[2][0])*dt;

A[STATE_X][STATE_D2] = (S[STATE_PX]*R[0][1] - S[STATE_PY]*R[0][0])*dt;
A[STATE_Y][STATE_D2] = (S[STATE_PX]*R[1][1] - S[STATE_PY]*R[1][0])*dt;
A[STATE_Z][STATE_D2] = (S[STATE_PX]*R[2][1] - S[STATE_PY]*R[2][0])*dt;

// body-frame velocity from body-frame velocity
A[STATE_PX][STATE_PX] = 1; //drag negligible
A[STATE_PY][STATE_PX] =-gyro->z*dt;
A[STATE_PZ][STATE_PX] = gyro->y*dt;

A[STATE_PX][STATE_PY] = gyro->z*dt;
A[STATE_PY][STATE_PY] = 1; //drag negligible
A[STATE_PZ][STATE_PY] =-gyro->x*dt;

A[STATE_PX][STATE_PZ] =-gyro->y*dt;
A[STATE_PY][STATE_PZ] = gyro->x*dt;
A[STATE_PZ][STATE_PZ] = 1; //drag negligible

// body-frame velocity from attitude error
A[STATE_PX][STATE_D0] = 0;
A[STATE_PY][STATE_D0] = -GRAVITY_MAGNITUDE*R[2][2]*dt;
A[STATE_PZ][STATE_D0] = GRAVITY_MAGNITUDE*R[2][1]*dt;

A[STATE_PX][STATE_D1] = GRAVITY_MAGNITUDE*R[2][2]*dt;
A[STATE_PY][STATE_D1] = 0;
A[STATE_PZ][STATE_D1] = -GRAVITY_MAGNITUDE*R[2][0]*dt;

A[STATE_PX][STATE_D2] = -GRAVITY_MAGNITUDE*R[2][1]*dt;
A[STATE_PY][STATE_D2] = GRAVITY_MAGNITUDE*R[2][0]*dt;
A[STATE_PZ][STATE_D2] = 0;

// attitude error from attitude error
/**
* At first glance, it may not be clear where the next values come from, since they do not appear directly in the
* dynamics. In this prediction step, we skip the step of first updating attitude-error, and then incorporating the
* new error into the current attitude (which requires a rotation of the attitude-error covariance). Instead, we
* directly update the body attitude, however still need to rotate the covariance, which is what you see below.
*
* This comes from a second order approximation to:
* Sigma_post = exps(-d) Sigma_pre exps(-d)'
* ~ (I + [[-d]] + [[-d]]^2 / 2) Sigma_pre (I + [[-d]] + [[-d]]^2 / 2)'
* where d is the attitude error expressed as Rodriges parameters, ie. d0 = 1/2*gyro.x*dt under the assumption that
* d = [0,0,0] at the beginning of each prediction step and that gyro.x is constant over the sampling period
*
* As derived in "Covariance Correction Step for Kalman Filtering with an Attitude"
* http://arc.aiaa.org/doi/abs/10.2514/1.G000848
*/
float d0 = gyro->x*dt/2;
float d1 = gyro->y*dt/2;
float d2 = gyro->z*dt/2;

A[STATE_D0][STATE_D0] = 1 - d1*d1/2 - d2*d2/2;
A[STATE_D0][STATE_D1] = d2 + d0*d1/2;
A[STATE_D0][STATE_D2] = -d1 + d0*d2/2;

A[STATE_D1][STATE_D0] = -d2 + d0*d1/2;
A[STATE_D1][STATE_D1] = 1 - d0*d0/2 - d2*d2/2;
A[STATE_D1][STATE_D2] = d0 + d1*d2/2;

A[STATE_D2][STATE_D0] = d1 + d0*d2/2;
A[STATE_D2][STATE_D1] = -d0 + d1*d2/2;
A[STATE_D2][STATE_D2] = 1 - d0*d0/2 - d1*d1/2;

[arnaud]

Hi,

I am not sure to understand your question, this looks like the Kalman filter Crazyflie is running, what do you mean by correct? Are you suspecting something is wrong?

As a side note, when copy-pasting code you can put it in code-block in the forum this way the code does not take so much space and will retain formating. An even better way in this case could have been to link to the code in github, to make it clear that you are referring to the code of the firmware and not to a modified version.

[AHMAD SHARIFA]

Dear arnaud ,

I do not suspect that the code is wrong, in control engineering we represent any dynamic system by state space representation, I have read many thesis that they used Crazyflie as a platform in their research, but no thesis has undertaken the same Matrices that has been used in the firmware. I will repeat my question in another context, I want to find the system Matrix A, in the c code in the firmware and the input matrix B in the c code in the firmware, is the part of the code that I have posted in my first question is what I am looking for??

Regards