state space of Crazyflie 2.0
Posted: Fri Apr 27, 2018 9:03 am
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;
// ====== 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;