mirror of
https://gitee.com/mirrors_PX4/PX4-Autopilot.git
synced 2026-06-28 03:50:34 +08:00
mathlib fixes
This commit is contained in:
Anton Babushkin
@@ -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)) {
|
||||
|
||||
@@ -125,17 +125,17 @@ public:
|
||||
virtual void updateParams();
|
||||
protected:
|
||||
// kalman filter
|
||||
math::Matrix F; /**< Jacobian(f,x), where dx/dt = f(x,u) */
|
||||
math::Matrix G; /**< noise shaping matrix for gyro/accel */
|
||||
math::Matrix P; /**< state covariance matrix */
|
||||
math::Matrix P0; /**< initial state covariance matrix */
|
||||
math::Matrix V; /**< gyro/ accel noise matrix */
|
||||
math::Matrix HAtt; /**< attitude measurement matrix */
|
||||
math::Matrix RAtt; /**< attitude measurement noise matrix */
|
||||
math::Matrix HPos; /**< position measurement jacobian matrix */
|
||||
math::Matrix RPos; /**< position measurement noise matrix */
|
||||
math::Matrix<9,9> F; /**< Jacobian(f,x), where dx/dt = f(x,u) */
|
||||
math::Matrix<9,6> G; /**< noise shaping matrix for gyro/accel */
|
||||
math::Matrix<9,9> P; /**< state covariance matrix */
|
||||
math::Matrix<9,9> P0; /**< initial state covariance matrix */
|
||||
math::Matrix<6,6> V; /**< gyro/ accel noise matrix */
|
||||
math::Matrix<4,9> HAtt; /**< attitude measurement matrix */
|
||||
math::Matrix<4,4> RAtt; /**< attitude measurement noise matrix */
|
||||
math::Matrix<6,9> HPos; /**< position measurement jacobian matrix */
|
||||
math::Matrix<6,6> RPos; /**< position measurement noise matrix */
|
||||
// attitude
|
||||
math::Dcm C_nb; /**< direction cosine matrix from body to nav frame */
|
||||
math::Matrix<3,3> C_nb; /**< direction cosine matrix from body to nav frame */
|
||||
math::Quaternion q; /**< quaternion from body to nav frame */
|
||||
// subscriptions
|
||||
control::UOrbSubscription<sensor_combined_s> _sensors; /**< sensors sub. */
|
||||
|
||||
@@ -194,15 +194,13 @@ int do_accel_calibration(int mavlink_fd)
|
||||
int32_t board_rotation_int;
|
||||
param_get(board_rotation_h, &(board_rotation_int));
|
||||
enum Rotation board_rotation_id = (enum Rotation)board_rotation_int;
|
||||
math::Matrix board_rotation(3, 3);
|
||||
math::Matrix<3,3> board_rotation;
|
||||
get_rot_matrix(board_rotation_id, &board_rotation);
|
||||
math::Matrix board_rotation_t = board_rotation.transpose();
|
||||
math::Vector3 accel_offs_vec;
|
||||
accel_offs_vec.set(&accel_offs[0]);
|
||||
math::Vector3 accel_offs_rotated = board_rotation_t * accel_offs_vec;
|
||||
math::Matrix accel_T_mat(3, 3);
|
||||
accel_T_mat.set(&accel_T[0][0]);
|
||||
math::Matrix accel_T_rotated = board_rotation_t * accel_T_mat * board_rotation;
|
||||
math::Matrix<3,3> board_rotation_t = board_rotation.transposed();
|
||||
math::Vector<3> accel_offs_vec(&accel_offs[0]);
|
||||
math::Vector<3> accel_offs_rotated = board_rotation_t * accel_offs_vec;
|
||||
math::Matrix<3,3> accel_T_mat(&accel_T[0][0]);
|
||||
math::Matrix<3,3> accel_T_rotated = board_rotation_t * accel_T_mat * board_rotation;
|
||||
|
||||
accel_scale.x_offset = accel_offs_rotated(0);
|
||||
accel_scale.x_scale = accel_T_rotated(0, 0);
|
||||
|
||||
@@ -683,8 +683,9 @@ handle_message(mavlink_message_t *msg)
|
||||
|
||||
/* Calculate Rotation Matrix */
|
||||
math::Quaternion q(hil_state.attitude_quaternion);
|
||||
math::Dcm C_nb(q);
|
||||
math::EulerAngles euler(C_nb);
|
||||
math::Matrix<3,3> C_nb;
|
||||
C_nb.from_quaternion(q);
|
||||
math::Vector<3> euler = C_nb.to_euler();
|
||||
|
||||
/* set rotation matrix */
|
||||
for (int i = 0; i < 3; i++) for (int j = 0; j < 3; j++)
|
||||
@@ -699,9 +700,9 @@ handle_message(mavlink_message_t *msg)
|
||||
hil_attitude.q[3] = q(3);
|
||||
hil_attitude.q_valid = true;
|
||||
|
||||
hil_attitude.roll = euler.getPhi();
|
||||
hil_attitude.pitch = euler.getTheta();
|
||||
hil_attitude.yaw = euler.getPsi();
|
||||
hil_attitude.roll = euler(0);
|
||||
hil_attitude.pitch = euler(1);
|
||||
hil_attitude.yaw = euler(2);
|
||||
hil_attitude.rollspeed = hil_state.rollspeed;
|
||||
hil_attitude.pitchspeed = hil_state.pitchspeed;
|
||||
hil_attitude.yawspeed = hil_state.yawspeed;
|
||||
|
||||
@@ -422,11 +422,11 @@ Navigator::task_main()
|
||||
|
||||
mission_poll();
|
||||
|
||||
math::Vector2f ground_speed(_global_pos.vx, _global_pos.vy);
|
||||
math::Vector<2> ground_speed(_global_pos.vx, _global_pos.vy);
|
||||
// Current waypoint
|
||||
math::Vector2f next_wp(_global_triplet.current.lat / 1e7f, _global_triplet.current.lon / 1e7f);
|
||||
math::Vector<2> next_wp(_global_triplet.current.lat / 1e7f, _global_triplet.current.lon / 1e7f);
|
||||
// Global position
|
||||
math::Vector2f current_position(_global_pos.lat / 1e7f, _global_pos.lon / 1e7f);
|
||||
math::Vector<2> current_position(_global_pos.lat / 1e7f, _global_pos.lon / 1e7f);
|
||||
|
||||
/* AUTONOMOUS FLIGHT */
|
||||
|
||||
@@ -436,19 +436,19 @@ Navigator::task_main()
|
||||
if (_mission_valid) {
|
||||
|
||||
// Next waypoint
|
||||
math::Vector2f prev_wp;
|
||||
math::Vector<2> prev_wp;
|
||||
|
||||
if (_global_triplet.previous_valid) {
|
||||
prev_wp.setX(_global_triplet.previous.lat / 1e7f);
|
||||
prev_wp.setY(_global_triplet.previous.lon / 1e7f);
|
||||
prev_wp(0) = _global_triplet.previous.lat / 1e7f;
|
||||
prev_wp(1) = _global_triplet.previous.lon / 1e7f;
|
||||
|
||||
} else {
|
||||
/*
|
||||
* No valid next waypoint, go for heading hold.
|
||||
* This is automatically handled by the L1 library.
|
||||
*/
|
||||
prev_wp.setX(_global_triplet.current.lat / 1e7f);
|
||||
prev_wp.setY(_global_triplet.current.lon / 1e7f);
|
||||
prev_wp(0) = _global_triplet.current.lat / 1e7f;
|
||||
prev_wp(1) = _global_triplet.current.lon / 1e7f;
|
||||
|
||||
}
|
||||
|
||||
|
||||
@@ -211,8 +211,8 @@ private:
|
||||
struct differential_pressure_s _diff_pres;
|
||||
struct airspeed_s _airspeed;
|
||||
|
||||
math::Matrix _board_rotation; /**< rotation matrix for the orientation that the board is mounted */
|
||||
math::Matrix _external_mag_rotation; /**< rotation matrix for the orientation that an external mag is mounted */
|
||||
math::Matrix<3,3> _board_rotation; /**< rotation matrix for the orientation that the board is mounted */
|
||||
math::Matrix<3,3> _external_mag_rotation; /**< rotation matrix for the orientation that an external mag is mounted */
|
||||
bool _mag_is_external; /**< true if the active mag is on an external board */
|
||||
|
||||
struct {
|
||||
@@ -457,8 +457,8 @@ Sensors::Sensors() :
|
||||
/* performance counters */
|
||||
_loop_perf(perf_alloc(PC_ELAPSED, "sensor task update")),
|
||||
|
||||
_board_rotation(3, 3),
|
||||
_external_mag_rotation(3, 3),
|
||||
_board_rotation(),
|
||||
_external_mag_rotation(),
|
||||
_mag_is_external(false)
|
||||
{
|
||||
|
||||
@@ -904,7 +904,7 @@ Sensors::accel_poll(struct sensor_combined_s &raw)
|
||||
|
||||
orb_copy(ORB_ID(sensor_accel), _accel_sub, &accel_report);
|
||||
|
||||
math::Vector3 vect = {accel_report.x, accel_report.y, accel_report.z};
|
||||
math::Vector<3> vect = {accel_report.x, accel_report.y, accel_report.z};
|
||||
vect = _board_rotation * vect;
|
||||
|
||||
raw.accelerometer_m_s2[0] = vect(0);
|
||||
@@ -930,7 +930,7 @@ Sensors::gyro_poll(struct sensor_combined_s &raw)
|
||||
|
||||
orb_copy(ORB_ID(sensor_gyro), _gyro_sub, &gyro_report);
|
||||
|
||||
math::Vector3 vect = {gyro_report.x, gyro_report.y, gyro_report.z};
|
||||
math::Vector<3> vect = {gyro_report.x, gyro_report.y, gyro_report.z};
|
||||
vect = _board_rotation * vect;
|
||||
|
||||
raw.gyro_rad_s[0] = vect(0);
|
||||
@@ -956,7 +956,7 @@ Sensors::mag_poll(struct sensor_combined_s &raw)
|
||||
|
||||
orb_copy(ORB_ID(sensor_mag), _mag_sub, &mag_report);
|
||||
|
||||
math::Vector3 vect = {mag_report.x, mag_report.y, mag_report.z};
|
||||
math::Vector<3> vect(mag_report.x, mag_report.y, mag_report.z);
|
||||
|
||||
if (_mag_is_external)
|
||||
vect = _external_mag_rotation * vect;
|
||||
|
||||
Reference in New Issue
Block a user