forked from Archive/PX4-Autopilot
Merge branch 'mag-correct-yaw' of github.com:jgoppert/Firmware into fw_hil_test
This commit is contained in:
Lorenz Meier
commit
582ee13d2a
|
|
@ -58,8 +58,8 @@ KalmanNav::KalmanNav(SuperBlock *parent, const char *name) :
|
|||
P0(9, 9),
|
||||
V(6, 6),
|
||||
// attitude measurement ekf matrices
|
||||
HAtt(6, 9),
|
||||
RAtt(6, 6),
|
||||
HAtt(4, 9),
|
||||
RAtt(4, 4),
|
||||
// position measurement ekf matrices
|
||||
HPos(6, 9),
|
||||
RPos(6, 6),
|
||||
|
|
@ -486,20 +486,10 @@ int KalmanNav::correctAtt()
|
|||
float sinPsi = sinf(psi);
|
||||
|
||||
// mag measurement
|
||||
Vector3 zMag(_sensors.magnetometer_ga);
|
||||
//float magNorm = zMag.norm();
|
||||
zMag = zMag.unit();
|
||||
|
||||
// mag predicted measurement
|
||||
// choosing some typical magnetic field properties,
|
||||
// TODO dip/dec depend on lat/ lon/ time
|
||||
float dip = _magDip.get() / M_RAD_TO_DEG_F; // dip, inclination with level
|
||||
float dec = _magDec.get() / M_RAD_TO_DEG_F; // declination, clockwise rotation from north
|
||||
float bN = cosf(dip) * cosf(dec);
|
||||
float bE = cosf(dip) * sinf(dec);
|
||||
float bD = sinf(dip);
|
||||
Vector3 bNav(bN, bE, bD);
|
||||
Vector3 zMagHat = (C_nb.transpose() * bNav).unit();
|
||||
Vector3 magNav = C_nb.transpose() * Vector3(_sensors.magnetometer_ga);
|
||||
float yMag = atan2f(magNav(0),magNav(1)) - psi;
|
||||
if (yMag > M_PI_F) yMag -= 2*M_PI_F;
|
||||
if (yMag < -M_PI_F) yMag += 2*M_PI_F;
|
||||
|
||||
// accel measurement
|
||||
Vector3 zAccel(_sensors.accelerometer_m_s2);
|
||||
|
|
@ -512,9 +502,9 @@ int KalmanNav::correctAtt()
|
|||
bool ignoreAccel = fabsf(accelMag - _g.get()) > 1.1f;
|
||||
|
||||
if (ignoreAccel) {
|
||||
RAttAdjust(1, 1) = 1.0e10;
|
||||
RAttAdjust(2, 2) = 1.0e10;
|
||||
RAttAdjust(3, 3) = 1.0e10;
|
||||
RAttAdjust(4, 4) = 1.0e10;
|
||||
RAttAdjust(5, 5) = 1.0e10;
|
||||
|
||||
} else {
|
||||
//printf("correcting attitude with accel\n");
|
||||
|
|
@ -523,58 +513,25 @@ int KalmanNav::correctAtt()
|
|||
// accel predicted measurement
|
||||
Vector3 zAccelHat = (C_nb.transpose() * Vector3(0, 0, -_g.get())).unit();
|
||||
|
||||
// combined measurement
|
||||
Vector zAtt(6);
|
||||
Vector zAttHat(6);
|
||||
// calculate residual
|
||||
Vector y(4);
|
||||
y(0) = yMag;
|
||||
y(1) = zAccel(0) - zAccelHat(0);
|
||||
y(2) = zAccel(1) - zAccelHat(1);
|
||||
y(3) = zAccel(2) - zAccelHat(2);
|
||||
|
||||
for (int i = 0; i < 3; i++) {
|
||||
zAtt(i) = zMag(i);
|
||||
zAtt(i + 3) = zAccel(i);
|
||||
zAttHat(i) = zMagHat(i);
|
||||
zAttHat(i + 3) = zAccelHat(i);
|
||||
}
|
||||
// HMag
|
||||
HAtt(0, 2) = 1;
|
||||
|
||||
// HMag , HAtt (0-2,:)
|
||||
float tmp1 =
|
||||
cosPsi * cosTheta * bN +
|
||||
sinPsi * cosTheta * bE -
|
||||
sinTheta * bD;
|
||||
HAtt(0, 1) = -(
|
||||
cosPsi * sinTheta * bN +
|
||||
sinPsi * sinTheta * bE +
|
||||
cosTheta * bD
|
||||
);
|
||||
HAtt(0, 2) = -cosTheta * (sinPsi * bN - cosPsi * bE);
|
||||
HAtt(1, 0) =
|
||||
(cosPhi * cosPsi * sinTheta + sinPhi * sinPsi) * bN +
|
||||
(cosPhi * sinPsi * sinTheta - sinPhi * cosPsi) * bE +
|
||||
cosPhi * cosTheta * bD;
|
||||
HAtt(1, 1) = sinPhi * tmp1;
|
||||
HAtt(1, 2) = -(
|
||||
(sinPhi * sinPsi * sinTheta + cosPhi * cosPsi) * bN -
|
||||
(sinPhi * cosPsi * sinTheta - cosPhi * sinPsi) * bE
|
||||
);
|
||||
HAtt(2, 0) = -(
|
||||
(sinPhi * cosPsi * sinTheta - cosPhi * sinPsi) * bN +
|
||||
(sinPhi * sinPsi * sinTheta + cosPhi * cosPsi) * bE +
|
||||
(sinPhi * cosTheta) * bD
|
||||
);
|
||||
HAtt(2, 1) = cosPhi * tmp1;
|
||||
HAtt(2, 2) = -(
|
||||
(cosPhi * sinPsi * sinTheta - sinPhi * cosTheta) * bN -
|
||||
(cosPhi * cosPsi * sinTheta + sinPhi * sinPsi) * bE
|
||||
);
|
||||
|
||||
// HAccel , HAtt (3-5,:)
|
||||
HAtt(3, 1) = cosTheta;
|
||||
HAtt(4, 0) = -cosPhi * cosTheta;
|
||||
HAtt(4, 1) = sinPhi * sinTheta;
|
||||
HAtt(5, 0) = sinPhi * cosTheta;
|
||||
HAtt(5, 1) = cosPhi * sinTheta;
|
||||
// HAccel
|
||||
HAtt(1, 1) = cosTheta;
|
||||
HAtt(2, 0) = -cosPhi * cosTheta;
|
||||
HAtt(2, 1) = sinPhi * sinTheta;
|
||||
HAtt(3, 0) = sinPhi * cosTheta;
|
||||
HAtt(3, 1) = cosPhi * sinTheta;
|
||||
|
||||
// compute correction
|
||||
// http://en.wikipedia.org/wiki/Extended_Kalman_filter
|
||||
Vector y = zAtt - zAttHat; // residual
|
||||
Matrix S = HAtt * P * HAtt.transpose() + RAttAdjust; // residual covariance
|
||||
Matrix K = P * HAtt.transpose() * S.inverse();
|
||||
Vector xCorrect = K * y;
|
||||
|
|
@ -617,9 +574,6 @@ int KalmanNav::correctAtt()
|
|||
if (beta > _faultAtt.get()) {
|
||||
printf("fault in attitude: beta = %8.4f\n", (double)beta);
|
||||
printf("y:\n"); y.print();
|
||||
printf("zMagHat:\n"); zMagHat.print();
|
||||
printf("zMag:\n"); zMag.print();
|
||||
printf("bNav:\n"); bNav.print();
|
||||
}
|
||||
|
||||
// update quaternions from euler
|
||||
|
|
@ -722,8 +676,6 @@ void KalmanNav::updateParams()
|
|||
if (noiseMagSq < noiseMin) noiseMagSq = noiseMin;
|
||||
|
||||
RAtt(0, 0) = noiseMagSq; // normalized direction
|
||||
RAtt(1, 1) = noiseMagSq;
|
||||
RAtt(2, 2) = noiseMagSq;
|
||||
|
||||
// accelerometer noise
|
||||
float noiseAccelSq = _rAccel.get() * _rAccel.get();
|
||||
|
|
@ -731,9 +683,9 @@ void KalmanNav::updateParams()
|
|||
// bound noise to prevent singularities
|
||||
if (noiseAccelSq < noiseMin) noiseAccelSq = noiseMin;
|
||||
|
||||
RAtt(3, 3) = noiseAccelSq; // normalized direction
|
||||
RAtt(4, 4) = noiseAccelSq;
|
||||
RAtt(5, 5) = noiseAccelSq;
|
||||
RAtt(1, 1) = noiseAccelSq; // normalized direction
|
||||
RAtt(2, 2) = noiseAccelSq;
|
||||
RAtt(3, 3) = noiseAccelSq;
|
||||
|
||||
// gps noise
|
||||
float R = R0 + float(alt);
|
||||
|
|
|
|||
Loading…
Reference in New Issue