diff options
Diffstat (limited to 'src/modules/att_pos_estimator_ekf/KalmanNav.cpp')
| -rw-r--r-- | src/modules/att_pos_estimator_ekf/KalmanNav.cpp | 812 |
1 files changed, 812 insertions, 0 deletions
diff --git a/src/modules/att_pos_estimator_ekf/KalmanNav.cpp b/src/modules/att_pos_estimator_ekf/KalmanNav.cpp new file mode 100644 index 000000000..5cf84542b --- /dev/null +++ b/src/modules/att_pos_estimator_ekf/KalmanNav.cpp @@ -0,0 +1,812 @@ +/**************************************************************************** + * + * Copyright (c) 2012, 2013 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +/** + * @file KalmanNav.cpp + * + * Kalman filter navigation code + */ + +#include <poll.h> + +#include "KalmanNav.hpp" +#include <systemlib/err.h> +#include <geo/geo.h> + +// constants +// Titterton pg. 52 +static const float omega = 7.2921150e-5f; // earth rotation rate, rad/s +static const float R0 = 6378137.0f; // earth radius, m +static const float g0 = 9.806f; // standard gravitational accel. m/s^2 +static const int8_t ret_ok = 0; // no error in function +static const int8_t ret_error = -1; // error occurred + +KalmanNav::KalmanNav(SuperBlock *parent, const char *name) : + SuperBlock(parent, name), + // subscriptions + _sensors(&getSubscriptions(), ORB_ID(sensor_combined), 5), // limit to 200 Hz + _gps(&getSubscriptions(), ORB_ID(vehicle_gps_position), 100), // limit to 10 Hz + _param_update(&getSubscriptions(), ORB_ID(parameter_update), 1000), // limit to 1 Hz + // publications + _pos(&getPublications(), ORB_ID(vehicle_global_position)), + _localPos(&getPublications(), ORB_ID(vehicle_local_position)), + _att(&getPublications(), ORB_ID(vehicle_attitude)), + // timestamps + _pubTimeStamp(hrt_absolute_time()), + _predictTimeStamp(hrt_absolute_time()), + _attTimeStamp(hrt_absolute_time()), + _outTimeStamp(hrt_absolute_time()), + // frame count + _navFrames(0), + // miss counts + _miss(0), + // accelerations + fN(0), fE(0), fD(0), + // state + phi(0), theta(0), psi(0), + vN(0), vE(0), vD(0), + lat(0), lon(0), alt(0), + lat0(0), lon0(0), alt0(0), + // parameters for ground station + _vGyro(this, "V_GYRO"), + _vAccel(this, "V_ACCEL"), + _rMag(this, "R_MAG"), + _rGpsVel(this, "R_GPS_VEL"), + _rGpsPos(this, "R_GPS_POS"), + _rGpsAlt(this, "R_GPS_ALT"), + _rPressAlt(this, "R_PRESS_ALT"), + _rAccel(this, "R_ACCEL"), + _magDip(this, "ENV_MAG_DIP"), + _magDec(this, "ENV_MAG_DEC"), + _g(this, "ENV_G"), + _faultPos(this, "FAULT_POS"), + _faultAtt(this, "FAULT_ATT"), + _attitudeInitialized(false), + _positionInitialized(false), + _attitudeInitCounter(0) +{ + using namespace math; + + memset(&ref, 0, sizeof(ref)); + + F.zero(); + G.zero(); + V.zero(); + HAtt.zero(); + RAtt.zero(); + HPos.zero(); + RPos.zero(); + + // initial state covariance matrix + P0.identity(); + P0 *= 0.01f; + P = P0; + + // initial state + phi = 0.0f; + theta = 0.0f; + psi = 0.0f; + vN = 0.0f; + vE = 0.0f; + vD = 0.0f; + lat = 0.0f; + lon = 0.0f; + alt = 0.0f; + + // initialize rotation quaternion with a single raw sensor measurement + _sensors.update(); + q = init( + _sensors.accelerometer_m_s2[0], + _sensors.accelerometer_m_s2[1], + _sensors.accelerometer_m_s2[2], + _sensors.magnetometer_ga[0], + _sensors.magnetometer_ga[1], + _sensors.magnetometer_ga[2]); + + // initialize dcm + C_nb = q.to_dcm(); + + // HPos is constant + HPos(0, 3) = 1.0f; + HPos(1, 4) = 1.0f; + HPos(2, 6) = 1.0e7f * M_RAD_TO_DEG_F; + HPos(3, 7) = 1.0e7f * M_RAD_TO_DEG_F; + HPos(4, 8) = 1.0f; + HPos(5, 8) = 1.0f; + + // initialize all parameters + updateParams(); +} + +math::Quaternion KalmanNav::init(float ax, float ay, float az, float mx, float my, float mz) +{ + float initialRoll, initialPitch; + float cosRoll, sinRoll, cosPitch, sinPitch; + float magX, magY; + float initialHdg, cosHeading, sinHeading; + + initialRoll = atan2(-ay, -az); + initialPitch = atan2(ax, -az); + + cosRoll = cosf(initialRoll); + sinRoll = sinf(initialRoll); + cosPitch = cosf(initialPitch); + sinPitch = sinf(initialPitch); + + magX = mx * cosPitch + my * sinRoll * sinPitch + mz * cosRoll * sinPitch; + + magY = my * cosRoll - mz * sinRoll; + + initialHdg = atan2f(-magY, magX); + + cosRoll = cosf(initialRoll * 0.5f); + sinRoll = sinf(initialRoll * 0.5f); + + cosPitch = cosf(initialPitch * 0.5f); + sinPitch = sinf(initialPitch * 0.5f); + + cosHeading = cosf(initialHdg * 0.5f); + sinHeading = sinf(initialHdg * 0.5f); + + float q0 = cosRoll * cosPitch * cosHeading + sinRoll * sinPitch * sinHeading; + float q1 = sinRoll * cosPitch * cosHeading - cosRoll * sinPitch * sinHeading; + float q2 = cosRoll * sinPitch * cosHeading + sinRoll * cosPitch * sinHeading; + float q3 = cosRoll * cosPitch * sinHeading - sinRoll * sinPitch * cosHeading; + + return math::Quaternion(q0, q1, q2, q3); + +} + +void KalmanNav::update() +{ + using namespace math; + + struct pollfd fds[1]; + fds[0].fd = _sensors.getHandle(); + fds[0].events = POLLIN; + + // poll for new data + int ret = poll(fds, 1, 1000); + + if (ret < 0) { + // XXX this is seriously bad - should be an emergency + return; + + } else if (ret == 0) { // timeout + return; + } + + // get new timestamp + uint64_t newTimeStamp = hrt_absolute_time(); + + // check updated subscriptions + if (_param_update.updated()) updateParams(); + + bool gpsUpdate = _gps.updated(); + bool sensorsUpdate = _sensors.updated(); + + // get new information from subscriptions + // this clears update flag + updateSubscriptions(); + + // initialize attitude when sensors online + if (!_attitudeInitialized && sensorsUpdate) { + if (correctAtt() == ret_ok) _attitudeInitCounter++; + + if (_attitudeInitCounter > 100) { + warnx("initialized EKF attitude"); + warnx("phi: %8.4f, theta: %8.4f, psi: %8.4f", + double(phi), double(theta), double(psi)); + _attitudeInitialized = true; + } + } + + // initialize position when gps received + if (!_positionInitialized && + _attitudeInitialized && // wait for attitude first + gpsUpdate && + _gps.fix_type > 2 + //&& _gps.counter_pos_valid > 10 + ) { + vN = _gps.vel_n_m_s; + vE = _gps.vel_e_m_s; + vD = _gps.vel_d_m_s; + setLatDegE7(_gps.lat); + setLonDegE7(_gps.lon); + setAltE3(_gps.alt); + // set reference position for + // local position + lat0 = lat; + lon0 = lon; + alt0 = alt; + map_projection_init(&ref, lat0, lon0); + _positionInitialized = true; + warnx("initialized EKF state with GPS"); + warnx("vN: %8.4f, vE: %8.4f, vD: %8.4f, lat: %8.4f, lon: %8.4f, alt: %8.4f", + double(vN), double(vE), double(vD), + lat, lon, double(alt)); + } + + // prediction step + // using sensors timestamp so we can account for packet lag + float dt = (_sensors.timestamp - _predictTimeStamp) / 1.0e6f; + //printf("dt: %15.10f\n", double(dt)); + _predictTimeStamp = _sensors.timestamp; + + // don't predict if time greater than a second + if (dt < 1.0f) { + predictState(dt); + predictStateCovariance(dt); + // count fast frames + _navFrames += 1; + } + + // count times 100 Hz rate isn't met + if (dt > 0.01f) _miss++; + + // gps correction step + if (_positionInitialized && gpsUpdate) { + correctPos(); + } + + // attitude correction step + if (_attitudeInitialized // initialized + && sensorsUpdate // new data + && _sensors.timestamp - _attTimeStamp > 1e6 / 50 // 50 Hz + ) { + _attTimeStamp = _sensors.timestamp; + correctAtt(); + } + + // publication + if (newTimeStamp - _pubTimeStamp > 1e6 / 50) { // 50 Hz + _pubTimeStamp = newTimeStamp; + + updatePublications(); + } + + // output + if (newTimeStamp - _outTimeStamp > 10e6) { // 0.1 Hz + _outTimeStamp = newTimeStamp; + //printf("nav: %4d Hz, miss #: %4d\n", + // _navFrames / 10, _miss / 10); + _navFrames = 0; + _miss = 0; + } +} + +void KalmanNav::updatePublications() +{ + using namespace math; + + // global position publication + _pos.timestamp = _pubTimeStamp; + _pos.time_gps_usec = _gps.timestamp_position; + _pos.lat = lat * M_RAD_TO_DEG; + _pos.lon = lon * M_RAD_TO_DEG; + _pos.alt = float(alt); + _pos.vel_n = vN; + _pos.vel_e = vE; + _pos.vel_d = vD; + _pos.yaw = psi; + + // local position publication + float x; + float y; + bool landed = alt < (alt0 + 0.1); // XXX improve? + map_projection_project(&ref, lat, lon, &x, &y); + _localPos.timestamp = _pubTimeStamp; + _localPos.xy_valid = true; + _localPos.z_valid = true; + _localPos.v_xy_valid = true; + _localPos.v_z_valid = true; + _localPos.x = x; + _localPos.y = y; + _localPos.z = alt0 - alt; + _localPos.vx = vN; + _localPos.vy = vE; + _localPos.vz = vD; + _localPos.yaw = psi; + _localPos.xy_global = true; + _localPos.z_global = true; + _localPos.ref_timestamp = _pubTimeStamp; + _localPos.ref_lat = lat * M_RAD_TO_DEG; + _localPos.ref_lon = lon * M_RAD_TO_DEG; + _localPos.ref_alt = 0; + _localPos.landed = landed; + + // attitude publication + _att.timestamp = _pubTimeStamp; + _att.roll = phi; + _att.pitch = theta; + _att.yaw = psi; + _att.rollspeed = _sensors.gyro_rad_s[0]; + _att.pitchspeed = _sensors.gyro_rad_s[1]; + _att.yawspeed = _sensors.gyro_rad_s[2]; + // TODO, add gyro offsets to filter + _att.rate_offsets[0] = 0.0f; + _att.rate_offsets[1] = 0.0f; + _att.rate_offsets[2] = 0.0f; + + for (int i = 0; i < 3; i++) for (int j = 0; j < 3; j++) + _att.R[i][j] = C_nb(i, j); + + for (int i = 0; i < 4; i++) _att.q[i] = q(i); + + _att.R_valid = true; + _att.q_valid = true; + + // selectively update publications, + // do NOT call superblock do-all method + if (_positionInitialized) { + _pos.update(); + _localPos.update(); + } + + if (_attitudeInitialized) + _att.update(); +} + +int KalmanNav::predictState(float dt) +{ + using namespace math; + + // trig + float sinL = sinf(lat); + float cosL = cosf(lat); + float cosLSing = cosf(lat); + + // prevent singularity + if (fabsf(cosLSing) < 0.01f) { + if (cosLSing > 0) cosLSing = 0.01; + else cosLSing = -0.01; + } + + // attitude prediction + if (_attitudeInitialized) { + Vector<3> w(_sensors.gyro_rad_s); + + // attitude + q = q + q.derivative(w) * dt; + + // renormalize quaternion if needed + if (fabsf(q.length() - 1.0f) > 1e-4f) { + q.normalize(); + } + + // C_nb update + C_nb = q.to_dcm(); + + // euler update + Vector<3> euler = C_nb.to_euler(); + phi = euler.data[0]; + theta = euler.data[1]; + psi = euler.data[2]; + + // specific acceleration in nav frame + Vector<3> accelB(_sensors.accelerometer_m_s2); + Vector<3> accelN = C_nb * accelB; + fN = accelN(0); + fE = accelN(1); + fD = accelN(2); + } + + // position prediction + if (_positionInitialized) { + // neglects angular deflections in local gravity + // see Titerton pg. 70 + float R = R0 + float(alt); + float LDot = vN / R; + float lDot = vE / (cosLSing * R); + float rotRate = 2 * omega + lDot; + + // XXX position prediction using speed + float vNDot = fN - vE * rotRate * sinL + + vD * LDot; + float vDDot = fD - vE * rotRate * cosL - + vN * LDot + _g.get(); + float vEDot = fE + vN * rotRate * sinL + + vDDot * rotRate * cosL; + + // rectangular integration + vN += vNDot * dt; + vE += vEDot * dt; + vD += vDDot * dt; + lat += double(LDot * dt); + lon += double(lDot * dt); + alt += double(-vD * dt); + } + + return ret_ok; +} + +int KalmanNav::predictStateCovariance(float dt) +{ + using namespace math; + + // trig + float sinL = sinf(lat); + float cosL = cosf(lat); + float cosLSq = cosL * cosL; + float tanL = tanf(lat); + + // prepare for matrix + float R = R0 + float(alt); + float RSq = R * R; + + // F Matrix + // Titterton pg. 291 + + F(0, 1) = -(omega * sinL + vE * tanL / R); + F(0, 2) = vN / R; + F(0, 4) = 1.0f / R; + F(0, 6) = -omega * sinL; + F(0, 8) = -vE / RSq; + + F(1, 0) = omega * sinL + vE * tanL / R; + F(1, 2) = omega * cosL + vE / R; + F(1, 3) = -1.0f / R; + F(1, 8) = vN / RSq; + + F(2, 0) = -vN / R; + F(2, 1) = -omega * cosL - vE / R; + F(2, 4) = -tanL / R; + F(2, 6) = -omega * cosL - vE / (R * cosLSq); + F(2, 8) = vE * tanL / RSq; + + F(3, 1) = -fD; + F(3, 2) = fE; + F(3, 3) = vD / R; + F(3, 4) = -2 * (omega * sinL + vE * tanL / R); + F(3, 5) = vN / R; + F(3, 6) = -vE * (2 * omega * cosL + vE / (R * cosLSq)); + F(3, 8) = (vE * vE * tanL - vN * vD) / RSq; + + F(4, 0) = fD; + F(4, 2) = -fN; + F(4, 3) = 2 * omega * sinL + vE * tanL / R; + F(4, 4) = (vN * tanL + vD) / R; + F(4, 5) = 2 * omega * cosL + vE / R; + F(4, 6) = 2 * omega * (vN * cosL - vD * sinL) + + vN * vE / (R * cosLSq); + F(4, 8) = -vE * (vN * tanL + vD) / RSq; + + F(5, 0) = -fE; + F(5, 1) = fN; + F(5, 3) = -2 * vN / R; + F(5, 4) = -2 * (omega * cosL + vE / R); + F(5, 6) = 2 * omega * vE * sinL; + F(5, 8) = (vN * vN + vE * vE) / RSq; + + F(6, 3) = 1 / R; + F(6, 8) = -vN / RSq; + + F(7, 4) = 1 / (R * cosL); + F(7, 6) = vE * tanL / (R * cosL); + F(7, 8) = -vE / (cosL * RSq); + + F(8, 5) = -1; + + // G Matrix + // Titterton pg. 291 + G(0, 0) = -C_nb(0, 0); + G(0, 1) = -C_nb(0, 1); + G(0, 2) = -C_nb(0, 2); + G(1, 0) = -C_nb(1, 0); + G(1, 1) = -C_nb(1, 1); + G(1, 2) = -C_nb(1, 2); + G(2, 0) = -C_nb(2, 0); + G(2, 1) = -C_nb(2, 1); + G(2, 2) = -C_nb(2, 2); + + G(3, 3) = C_nb(0, 0); + G(3, 4) = C_nb(0, 1); + G(3, 5) = C_nb(0, 2); + G(4, 3) = C_nb(1, 0); + G(4, 4) = C_nb(1, 1); + G(4, 5) = C_nb(1, 2); + G(5, 3) = C_nb(2, 0); + G(5, 4) = C_nb(2, 1); + G(5, 5) = C_nb(2, 2); + + // continuous prediction equations + // for discrete time EKF + // http://en.wikipedia.org/wiki/Extended_Kalman_filter + P = P + (F * P + P * F.transposed() + G * V * G.transposed()) * dt; + + return ret_ok; +} + +int KalmanNav::correctAtt() +{ + using namespace math; + + // trig + float cosPhi = cosf(phi); + float cosTheta = cosf(theta); + // float cosPsi = cosf(psi); + float sinPhi = sinf(phi); + float sinTheta = sinf(theta); + // float sinPsi = sinf(psi); + + // 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 + + // compensate roll and pitch, but not yaw + // XXX take the vectors out of the C_nb matrix to avoid singularities + math::Matrix<3,3> C_rp; + C_rp.from_euler(phi, theta, 0.0f);//C_nb.transposed(); + + // mag measurement + Vector<3> magBody(_sensors.magnetometer_ga); + + // transform to earth frame + Vector<3> magNav = C_rp * magBody; + + // calculate error between estimate and measurement + // apply declination correction for true heading as well. + float yMag = -atan2f(magNav(1),magNav(0)) - psi - dec; + if (yMag > M_PI_F) yMag -= 2*M_PI_F; + if (yMag < -M_PI_F) yMag += 2*M_PI_F; + + // accel measurement + Vector<3> zAccel(_sensors.accelerometer_m_s2); + float accelMag = zAccel.length(); + zAccel.normalize(); + + // ignore accel correction when accel mag not close to g + Matrix<4,4> RAttAdjust = RAtt; + + 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; + + } else { + //printf("correcting attitude with accel\n"); + } + + // accel predicted measurement + Vector<3> zAccelHat = (C_nb.transposed() * Vector<3>(0, 0, -_g.get())).normalized(); + + // calculate residual + Vector<4> y(yMag, zAccel(0) - zAccelHat(0), zAccel(1) - zAccelHat(1), zAccel(2) - zAccelHat(2)); + + // HMag + HAtt(0, 2) = 1; + + // 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 + 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.get_size(); i++) { + float val = xCorrect(i); + + if (isnan(val) || isinf(val)) { + // abort correction and return + warnx("numerical failure in att correction"); + // reset P matrix to P0 + P = P0; + return ret_error; + } + } + + // correct state + if (!ignoreAccel) { + phi += xCorrect(PHI); + theta += xCorrect(THETA); + } + + psi += xCorrect(PSI); + + // attitude also affects nav velocities + if (_positionInitialized) { + vN += xCorrect(VN); + vE += xCorrect(VE); + vD += xCorrect(VD); + } + + // update state covariance + // http://en.wikipedia.org/wiki/Extended_Kalman_filter + P = P - K * HAtt * P; + + // fault detection + float beta = y * (S.inversed() * y); + + if (beta > _faultAtt.get()) { + warnx("fault in attitude: beta = %8.4f", (double)beta); + warnx("y:"); y.print(); + } + + // update quaternions from euler + // angle correction + q.from_euler(phi, theta, psi); + + return ret_ok; +} + +int KalmanNav::correctPos() +{ + using namespace math; + + // residual + 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; + y(3) = double(_gps.lon) - double(lon) * 1.0e7 * M_RAD_TO_DEG; + y(4) = _gps.alt / 1.0e3f - alt; + y(5) = _sensors.baro_alt_meter - alt; + + // compute correction + // http://en.wikipedia.org/wiki/Extended_Kalman_filter + 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.get_size(); i++) { + float val = xCorrect(i); + + if (!isfinite(val)) { + // abort correction and return + warnx("numerical failure in gps correction"); + // fallback to GPS + vN = _gps.vel_n_m_s; + vE = _gps.vel_e_m_s; + vD = _gps.vel_d_m_s; + setLatDegE7(_gps.lat); + setLonDegE7(_gps.lon); + setAltE3(_gps.alt); + // reset P matrix to P0 + P = P0; + return ret_error; + } + } + + // correct state + vN += xCorrect(VN); + vE += xCorrect(VE); + vD += xCorrect(VD); + lat += double(xCorrect(LAT)); + lon += double(xCorrect(LON)); + alt += xCorrect(ALT); + + // update state covariance + // http://en.wikipedia.org/wiki/Extended_Kalman_filter + P = P - K * HPos * P; + + // fault detetcion + float beta = y * (S.inversed() * y); + + static int counter = 0; + if (beta > _faultPos.get() && (counter % 10 == 0)) { + warnx("fault in gps: beta = %8.4f", (double)beta); + warnx("Y/N: vN: %8.4f, vE: %8.4f, lat: %8.4f, lon: %8.4f, alt: %8.4f, baro: %8.4f", + double(y(0) / sqrtf(RPos(0, 0))), + double(y(1) / sqrtf(RPos(1, 1))), + double(y(2) / sqrtf(RPos(2, 2))), + double(y(3) / sqrtf(RPos(3, 3))), + double(y(4) / sqrtf(RPos(4, 4))), + double(y(5) / sqrtf(RPos(5, 5)))); + } + counter++; + + return ret_ok; +} + +void KalmanNav::updateParams() +{ + using namespace math; + using namespace control; + SuperBlock::updateParams(); + + // gyro noise + V(0, 0) = _vGyro.get(); // gyro x, rad/s + V(1, 1) = _vGyro.get(); // gyro y + V(2, 2) = _vGyro.get(); // gyro z + + // accel noise + V(3, 3) = _vAccel.get(); // accel x, m/s^2 + V(4, 4) = _vAccel.get(); // accel y + V(5, 5) = _vAccel.get(); // accel z + + // magnetometer noise + float noiseMin = 1e-6f; + float noiseMagSq = _rMag.get() * _rMag.get(); + + if (noiseMagSq < noiseMin) noiseMagSq = noiseMin; + + RAtt(0, 0) = noiseMagSq; // normalized direction + + // accelerometer noise + float noiseAccelSq = _rAccel.get() * _rAccel.get(); + + // bound noise to prevent singularities + if (noiseAccelSq < noiseMin) noiseAccelSq = noiseMin; + + RAtt(1, 1) = noiseAccelSq; // normalized direction + RAtt(2, 2) = noiseAccelSq; + RAtt(3, 3) = noiseAccelSq; + + // gps noise + float R = R0 + float(alt); + float cosLSing = cosf(lat); + + // prevent singularity + if (fabsf(cosLSing) < 0.01f) { + if (cosLSing > 0) cosLSing = 0.01; + else cosLSing = -0.01; + } + + float noiseVel = _rGpsVel.get(); + float noiseLatDegE7 = 1.0e7f * M_RAD_TO_DEG_F * _rGpsPos.get() / R; + float noiseLonDegE7 = noiseLatDegE7 / cosLSing; + float noiseGpsAlt = _rGpsAlt.get(); + float noisePressAlt = _rPressAlt.get(); + + // bound noise to prevent singularities + if (noiseVel < noiseMin) noiseVel = noiseMin; + + if (noiseLatDegE7 < noiseMin) noiseLatDegE7 = noiseMin; + + if (noiseLonDegE7 < noiseMin) noiseLonDegE7 = noiseMin; + + if (noiseGpsAlt < noiseMin) noiseGpsAlt = noiseMin; + + if (noisePressAlt < noiseMin) noisePressAlt = noiseMin; + + RPos(0, 0) = noiseVel * noiseVel; // vn + RPos(1, 1) = noiseVel * noiseVel; // ve + RPos(2, 2) = noiseLatDegE7 * noiseLatDegE7; // lat + RPos(3, 3) = noiseLonDegE7 * noiseLonDegE7; // lon + RPos(4, 4) = noiseGpsAlt * noiseGpsAlt; // h + RPos(5, 5) = noisePressAlt * noisePressAlt; // h + // XXX, note that RPos depends on lat, so updateParams should + // be called if lat changes significantly +} |