paparazzi-devel
[Top][All Lists]

## [Paparazzi-devel] Question of EKF (Alex Yu)

 From: 于渊 Subject: [Paparazzi-devel] Question of EKF (Alex Yu) Date: Thu, 3 Feb 2011 14:20:31 +0800

Hi:
I reviewed the EKF algorithm implementation existed in paparazziw\sw\in_progress\inertial\C, when quaternion is used in the state space ,for example, ahrs_quat_ekf.c or ahrs_quat_fast_ekf.c. I find that the Jacobian of the measurements to the system states really confused me.
We know the Jacobian of the measurements to the system states is the derivate of measurements to the system states. and we know the derivative of atan(b(x)/a(x))=(b'a-a'b)/(a^2+b^2).
So let's check macro function "AFE_COMPUTE_H_PHI" defined in  the ahrs_quat_fast_ekf_utils.h for example.
we know the function of phi= arctan(2(e0e1+e2e3)/(e0^2-e1^2-e2^2+e3^2))
we can get the derivate of phi to e0,e1,e2,e3 to get the jacobian matrix
which i caculated is
#define AFE_COMPUTE_H_PHI() {           \
const FLOAT_T phi_err =  2 / (afe_dcm22*afe_dcm22 + afe_dcm12*afe_dcm12);     \
afe_H = (afe_q1 * afe_dcm22-afe_q0*afe_dcm12) * phi_err;         \
afe_H = (afe_q0 * afe_dcm22 +  afe_q1 * afe_dcm12) * phi_err;         \
afe_H = (afe_q3 * afe_dcm22 +afe_q2 * afe_dcm12) * phi_err;         \
afe_H = (afe_q2 * afe_dcm22-afe_q3*afe_dcm12) * phi_err;         \
}
while i found in a
hrs_quat_fast_ekf_utils.h  is
#define AFE_COMPUTE_H_PHI() {           \
const FLOAT_T phi_err =  2 / (afe_dcm22*afe_dcm22 + afe_dcm12*afe_dcm12);     \
afe_H = (afe_q1 * afe_dcm22) * phi_err;        \
afe_H = (afe_q0 * afe_dcm22 + 2 * afe_q1 * afe_dcm12) * phi_err;        \
afe_H = (afe_q3 * afe_dcm22 + 2 * afe_q2 * afe_dcm12) * phi_err;        \
afe_H = (afe_q2 * afe_dcm22) * phi_err;        \
}
So could anybody tell me what's wrong with me ?