fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-10-22 06:13:33 +08:00
Code Issues Packages Projects Releases Wiki Activity

mathlib fixes

Browse Source
This commit is contained in:
Anton Babushkin
2013-12-19 14:10:25 +04:00
parent e3a5a384d7
commit ba612c3ee8
22 changed files with 513 additions and 1191 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/att_pos_estimator_ekf/KalmanNav.cpp
+45 -47
View File
@@ -54,17 +54,17 @@ static const int8_t ret_error = -1; // error occurred
KalmanNav::KalmanNav(SuperBlock *parent, const char *name) :
SuperBlock(parent, name),
// ekf matrices
F(9, 9),
G(9, 6),
P(9, 9),
P0(9, 9),
V(6, 6),
F(),
G(),
P(),
P0(),
V(),
// attitude measurement ekf matrices
HAtt(4, 9),
RAtt(4, 4),
HAtt(),
RAtt(),
// position measurement ekf matrices
HPos(6, 9),
RPos(6, 6),
HPos(),
RPos(),
// attitude representations
C_nb(),
q(),
@@ -113,7 +113,8 @@ KalmanNav::KalmanNav(SuperBlock *parent, const char *name) :
using namespace math;
// initial state covariance matrix
P0 = Matrix::identity(9) * 0.01f;
P0.identity();
P0 *= 0.01f;
P = P0;
// initial state
@@ -138,7 +139,7 @@ KalmanNav::KalmanNav(SuperBlock *parent, const char *name) :
_sensors.magnetometer_ga[2]);
// initialize dcm
C_nb = Dcm(q);
C_nb.from_quaternion(q);
// HPos is constant
HPos(0, 3) = 1.0f;
@@ -404,28 +405,28 @@ int KalmanNav::predictState(float dt)
// attitude prediction
if (_attitudeInitialized) {
Vector3 w(_sensors.gyro_rad_s);
Vector<3> w(_sensors.gyro_rad_s);
// attitude
q = q + q.derivative(w) * dt;
// renormalize quaternion if needed
if (fabsf(q.norm() - 1.0f) > 1e-4f) {
q = q.unit();
if (fabsf(q.length() - 1.0f) > 1e-4f) {
q.normalize();
}
// C_nb update
C_nb = Dcm(q);
C_nb.from_quaternion(q);
// euler update
EulerAngles euler(C_nb);
phi = euler.getPhi();
theta = euler.getTheta();
psi = euler.getPsi();
Vector<3> euler = C_nb.to_euler();
phi = euler.data[0];
theta = euler.data[1];
psi = euler.data[2];
// specific acceleration in nav frame
Vector3 accelB(_sensors.accelerometer_m_s2);
Vector3 accelN = C_nb * accelB;
Vector<3> accelB(_sensors.accelerometer_m_s2);
Vector<3> accelN = C_nb * accelB;
fN = accelN(0);
fE = accelN(1);
fD = accelN(2);
@@ -549,10 +550,10 @@ int KalmanNav::predictStateCovariance(float dt)
G(5, 4) = C_nb(2, 1);
G(5, 5) = C_nb(2, 2);
// continuous predictioon equations
// for discrte time EKF
// continuous prediction equations
// for discrete time EKF
// http://en.wikipedia.org/wiki/Extended_Kalman_filter
P = P + (F * P + P * F.transpose() + G * V * G.transpose()) * dt;
P = P + (F * P + P * F.transposed() + G * V * G.transposed()) * dt;
return ret_ok;
}
@@ -577,13 +578,14 @@ int KalmanNav::correctAtt()
// compensate roll and pitch, but not yaw
// XXX take the vectors out of the C_nb matrix to avoid singularities
math::Dcm C_rp(math::EulerAngles(phi, theta, 0.0f));//C_nb.transpose();
math::Matrix<3,3> C_rp;
C_rp.from_euler(phi, theta, 0.0f);//C_nb.transposed();
// mag measurement
Vector3 magBody(_sensors.magnetometer_ga);
Vector<3> magBody(_sensors.magnetometer_ga);
// transform to earth frame
Vector3 magNav = C_rp * magBody;
Vector<3> magNav = C_rp * magBody;
// calculate error between estimate and measurement
// apply declination correction for true heading as well.
@@ -592,12 +594,12 @@ int KalmanNav::correctAtt()
if (yMag < -M_PI_F) yMag += 2*M_PI_F;
// accel measurement
Vector3 zAccel(_sensors.accelerometer_m_s2);
float accelMag = zAccel.norm();
zAccel = zAccel.unit();
Vector<3> zAccel(_sensors.accelerometer_m_s2);
float accelMag = zAccel.length();
zAccel.normalize();
// ignore accel correction when accel mag not close to g
Matrix RAttAdjust = RAtt;
Matrix<4,4> RAttAdjust = RAtt;
bool ignoreAccel = fabsf(accelMag - _g.get()) > 1.1f;
@@ -611,14 +613,10 @@ int KalmanNav::correctAtt()
}
// accel predicted measurement
Vector3 zAccelHat = (C_nb.transpose() * Vector3(0, 0, -_g.get())).unit();
Vector<3> zAccelHat = (C_nb.transposed() * Vector<3>(0, 0, -_g.get())).normalized();
// 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);
Vector<4> y(yMag, zAccel(0) - zAccelHat(0), zAccel(1) - zAccelHat(1), zAccel(2) - zAccelHat(2));
// HMag
HAtt(0, 2) = 1;
@@ -632,9 +630,9 @@ int KalmanNav::correctAtt()
// compute correction
// http://en.wikipedia.org/wiki/Extended_Kalman_filter
Matrix S = HAtt * P * HAtt.transpose() + RAttAdjust; // residual covariance
Matrix K = P * HAtt.transpose() * S.inverse();
Vector xCorrect = K * y;
Matrix<4, 4> S = HAtt * P * HAtt.transposed() + RAttAdjust; // residual covariance
Matrix<9, 4> K = P * HAtt.transposed() * S.inversed();
Vector<9> xCorrect = K * y;
// check correciton is sane
for (size_t i = 0; i < xCorrect.getRows(); i++) {
@@ -669,7 +667,7 @@ int KalmanNav::correctAtt()
P = P - K * HAtt * P;
// fault detection
float beta = y.dot(S.inverse() * y);
float beta = y * (S.inversed() * y);
if (beta > _faultAtt.get()) {
warnx("fault in attitude: beta = %8.4f", (double)beta);
@@ -678,7 +676,7 @@ int KalmanNav::correctAtt()
// update quaternions from euler
// angle correction
q = Quaternion(EulerAngles(phi, theta, psi));
q.from_euler(phi, theta, psi);
return ret_ok;
}
@@ -688,7 +686,7 @@ int KalmanNav::correctPos()
using namespace math;
// residual
Vector y(6);
Vector<6> y;
y(0) = _gps.vel_n_m_s - vN;
y(1) = _gps.vel_e_m_s - vE;
y(2) = double(_gps.lat) - double(lat) * 1.0e7 * M_RAD_TO_DEG;
@@ -698,9 +696,9 @@ int KalmanNav::correctPos()
// compute correction
// http://en.wikipedia.org/wiki/Extended_Kalman_filter
Matrix S = HPos * P * HPos.transpose() + RPos; // residual covariance
Matrix K = P * HPos.transpose() * S.inverse();
Vector xCorrect = K * y;
Matrix<6,6> S = HPos * P * HPos.transposed() + RPos; // residual covariance
Matrix<9,6> K = P * HPos.transposed() * S.inversed();
Vector<9> xCorrect = K * y;
// check correction is sane
for (size_t i = 0; i < xCorrect.getRows(); i++) {
@@ -735,7 +733,7 @@ int KalmanNav::correctPos()
P = P - K * HPos * P;
// fault detetcion
float beta = y.dot(S.inverse() * y);
float beta = y * (S.inversed() * y);
static int counter = 0;
if (beta > _faultPos.get() && (counter % 10 == 0)) {