[Top][All Lists]
[Date Prev][Date Next][Thread Prev][Thread Next][Date Index][Thread Index]
[paparazzi-commits] [5849] added libeknav
|
From: |
antoine drouin |
|
Subject: |
[paparazzi-commits] [5849] added libeknav |
|
Date: |
Fri, 10 Sep 2010 14:26:27 +0000 |
Revision: 5849
http://svn.sv.gnu.org/viewvc/?view=rev&root=paparazzi&revision=5849
Author: poine
Date: 2010-09-10 14:26:27 +0000 (Fri, 10 Sep 2010)
Log Message:
-----------
added libeknav
Added Paths:
-----------
paparazzi3/trunk/conf/airframes/Poine/fuckyou.xml
paparazzi3/trunk/sw/airborne/fms/libeknav/
paparazzi3/trunk/sw/airborne/fms/libeknav/basic_ins_qkf.cpp
paparazzi3/trunk/sw/airborne/fms/libeknav/hello_world.c
paparazzi3/trunk/sw/airborne/fms/libeknav/ins_qkf.hpp
paparazzi3/trunk/sw/airborne/fms/libeknav/ins_qkf_observe_gps_pvt.cpp
paparazzi3/trunk/sw/airborne/fms/libeknav/ins_qkf_observe_vector.cpp
paparazzi3/trunk/sw/airborne/fms/libeknav/ins_qkf_predict.cpp
paparazzi3/trunk/sw/airborne/fms/libeknav/quaternions.hpp
Added: paparazzi3/trunk/conf/airframes/Poine/fuckyou.xml
===================================================================
--- paparazzi3/trunk/conf/airframes/Poine/fuckyou.xml
(rev 0)
+++ paparazzi3/trunk/conf/airframes/Poine/fuckyou.xml 2010-09-10 14:26:27 UTC
(rev 5849)
@@ -0,0 +1,20 @@
+<airframe name="FuckYou_1">
+
+
+ <makefile>
+ HOST=auto3
+
+
+ # test 1
+ test42.ARCHDIR = omap
+ test42.CFLAGS += -I$(ACINCLUDE) -I. -I$(PAPARAZZI_HOME)/var/include
+ test42.srcs = fms/libeknav/hello_world.c
+# test1.srcs += fms/bla.c
+
+
+ </makefile>
+
+
+</airframe>
+
+
Added: paparazzi3/trunk/sw/airborne/fms/libeknav/basic_ins_qkf.cpp
===================================================================
--- paparazzi3/trunk/sw/airborne/fms/libeknav/basic_ins_qkf.cpp
(rev 0)
+++ paparazzi3/trunk/sw/airborne/fms/libeknav/basic_ins_qkf.cpp 2010-09-10
14:26:27 UTC (rev 5849)
@@ -0,0 +1,152 @@
+/*
+ * basic_ins_qkf.cpp
+ *
+ * Created on: Aug 11, 2009
+ * Author: Jonathan Brandmeyer
+ * This file is part of libeknav.
+ *
+ * Libeknav is free software: you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation, version 3.
+ *
+ * Libeknav is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ * GNU General Public License for more details.
+
+ * You should have received a copy of the GNU General Public License
+ * along with libeknav. If not, see <http://www.gnu.org/licenses/>.
+ *
+ */
+
+#include "ins_qkf.hpp"
+#include "assertions.hpp"
+
+using namespace Eigen;
+
+basic_ins_qkf::basic_ins_qkf(
+ const Vector3d& estimate,
+ double pos_error, double bias_error, double v_error,
+ const Vector3d& gyro_white_noise,
+ const Vector3d& gyro_stability_noise,
+ const Vector3d& accel_white_noise,
+ const Vector3d& vel_estimate)
+ : gyro_stability_noise(gyro_stability_noise)
+ , gyro_white_noise(gyro_white_noise)
+ , accel_white_noise(accel_white_noise)
+{
+ avg_state.position = estimate;
+ avg_state.gyro_bias = Vector3d::Zero();
+ avg_state.orientation = Quaterniond::Identity();
+ avg_state.velocity = vel_estimate;
+
+ cov << Matrix3d::Identity()*bias_error*bias_error, Matrix<double, 3,
9>::Zero(),
+ Matrix3d::Zero(), Matrix3d::Identity()*M_PI*M_PI*0.5,
Matrix<double, 3, 6>::Zero(),
+ Matrix<double, 3, 6>::Zero(),
Matrix3d::Identity()*pos_error*pos_error, Matrix3d::Zero(),
+ Matrix<double, 3, 9>::Zero(),
Matrix3d::Identity()*v_error*v_error;
+ assert(is_real());
+}
+
+void
+basic_ins_qkf::counter_rotate_cov(const Quaterniond&)
+{
+ // Rotate the principle axes of the angular error covariance by the
+ // mean update.
+
+ // TODO: This is only required in the case that the system covariance is
+ // right-multiplied by the mean. The current design left-multiplies the
+ // covariance by the mean.
+ return;
+
+ // TODO: There should be an expression that makes both this matrix's
+ // construction and multiplication much more efficient.
+ // Matrix<double, 12, 12> counter_rot = Matrix<double, 12,
12>::Identity();
+ // counter_rot.block<3, 3>(3, 3) =
update.cast<double>().conjugate().toRotationMatrix();
+ // cov = (counter_rot * cov.cast<double>() *
counter_rot.transpose()).cast<float>();
+}
+#if 0
+basic_ins_qkf::state
+basic_ins_qkf::average_sigma_points(const std::vector<state,
aligned_allocator<state> >& points)
+{
+ state ret;
+ Vector3d sum;
+#define avg_vector_field(field) \
+ sum = Vector3d::Zero(); \
+ for (auto i = points.begin(), end = points.end(); i != end; ++i) { \
+ sum += i->field; \
+ } \
+ ret.field = sum / points.size()
+
+ avg_vector_field(gyro_bias);
+ avg_vector_field(velocity);
+
+ Vector3d p_sum = Vector3d::Zero();
+ for (auto i = points.begin(), end = points.end(); i != end; ++i) {
+ p_sum += i->position;
+ }
+ ret.position = p_sum / (double)points.size();
+
+ std::vector<Quaterniond> quat_points;
+ quat_points.reserve(points.size());
+ for (auto i = points.begin(), end = points.end(); i != end; ++i) {
+ quat_points.push_back(i->orientation);
+ }
+ ret.orientation = quaternion_avg_johnson(quat_points).normalized();
+ return ret;
+}
+#endif
+
+bool
+basic_ins_qkf::is_real(void) const
+{
+ return !(hasNaN(cov) || hasInf(cov)) && avg_state.is_real();
+}
+
+Quaterniond
+basic_ins_qkf::state::apply_kalman_vec_update(const Matrix<double, 12, 1>
update)
+{
+ // std::cout << "***update available***\n"
+ // << "\tstate: "; print(std::cout);
+ // std::cout << "\n\tupdate: " << update.transpose() << "\n";
+ gyro_bias += update.segment<3>(0);
+ Quaterniond posterior_update = exp<double>(update.segment<3>(3));
+ orientation = (orientation * posterior_update).normalized();
+ position += update.segment<3>(6);
+ velocity += update.segment<3>(9);
+ assert(is_real());
+ return posterior_update;
+}
+
+#if 0
+Quaterniond
+basic_ins_qkf::state::apply_left_kalman_vec_update(const Matrix<double, 12, 1>
update)
+{
+ // std::cout << "***update available***\n"
+ // << "\tstate: "; print(std::cout);
+ // std::cout << "\n\tupdate: " << update.transpose() << "\n";
+ gyro_bias += update.segment<3>(0);
+ Quaterniond posterior_update = exp<double>(update.segment<3>(3));
+ orientation = (posterior_update * orientation).normalized();
+ position += update.segment<3>(6);
+ velocity += update.segment<3>(9);
+ assert(is_real());
+ return posterior_update;
+}
+#endif
+
+bool
+basic_ins_qkf::state::has_nan(void)const
+{
+ return hasNaN(gyro_bias) || hasNaN(orientation.coeffs())
+ || hasNaN(position) || hasNaN(velocity);
+}
+
+bool
+basic_ins_qkf::state::is_real(void) const
+{
+ return !(hasNaN(gyro_bias) || hasNaN(orientation.coeffs())
+ || hasNaN(position) || hasNaN(velocity))
+ && !(hasInf(gyro_bias) || hasInf(orientation.coeffs())
+ || hasInf(position) || hasInf(velocity));
+}
+
Added: paparazzi3/trunk/sw/airborne/fms/libeknav/hello_world.c
===================================================================
--- paparazzi3/trunk/sw/airborne/fms/libeknav/hello_world.c
(rev 0)
+++ paparazzi3/trunk/sw/airborne/fms/libeknav/hello_world.c 2010-09-10
14:26:27 UTC (rev 5849)
@@ -0,0 +1,12 @@
+
+
+#include <stdio.h>
+
+
+int main(int argc, char** argv) {
+
+ printf("hello world\n");
+
+ return 0;
+}
+
Added: paparazzi3/trunk/sw/airborne/fms/libeknav/ins_qkf.hpp
===================================================================
--- paparazzi3/trunk/sw/airborne/fms/libeknav/ins_qkf.hpp
(rev 0)
+++ paparazzi3/trunk/sw/airborne/fms/libeknav/ins_qkf.hpp 2010-09-10
14:26:27 UTC (rev 5849)
@@ -0,0 +1,286 @@
+#ifndef AHRS_INS_QKF_HPP
+#define AHRS_INS_QKF_HPP
+/*
+ * ins_qkf.cpp
+ *
+ * Author: Jonathan Brandmeyer
+ *
+ * This file is part of libeknav.
+ *
+ * Libeknav is free software: you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation, version 3.
+ *
+ * Libeknav is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ * GNU General Public License for more details.
+
+ * You should have received a copy of the GNU General Public License
+ * along with libeknav. If not, see <http://www.gnu.org/licenses/>.
+ */
+
+#include "sigma_points.hpp"
+#include "quaternions.hpp"
+#include <Eigen/StdVector>
+
+using Eigen::Vector3f;
+using Eigen::Vector3d;
+using Eigen::Vector2d;
+using Eigen::Quaterniond;
+using Eigen::Matrix;
+using Eigen::aligned_allocator;
+
+
+struct basic_ins_qkf
+{
+ EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+ /// The maximum number of satellites that may be tracked by the filter
+ static const size_t max_sv = 12;
+ /**
+ * The covariance of the zero-mean gaussian white noise that is added to
+ * the gyro bias at each time step. This value is treated as a diagonal
+ * matrix, in units of radians^2/second^2.
+ */
+ const Vector3d gyro_stability_noise;
+
+ /**
+ * The covariance of the zero-mean gaussian white noise that is added to
+ * the gyro measurement at each time step, in rad^2/second^2
+ */
+ const Vector3d gyro_white_noise;
+
+ /**
+ * The covariance of the zero-mean gaussian white noise that is added to
+ * the accelerometer measurement at each time step, in (m/s/s)^2
+ */
+ const Vector3d accel_white_noise;
+
+ /**
+ * A term for the basic state of the system
+ */
+ struct state
+ {
+ EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+ typedef std::vector<state, aligned_allocator<state> >
container_t;
+
+ /**
+ * Construct a state variable from mean and error terms
+ * @param mean The average state
+ * @param error An error vector of length 12 or greater
+ */
+ template <typename Vector_T>
+ state(const state& mean, const Vector_T& error)
+ : gyro_bias(mean.gyro_bias + error.template
segment<3>(0))
+ , orientation(mean.orientation *
exp<double>(error.template segment<3>(3)))
+ , position(mean.position + error.template
segment<3>(6).template cast<double>())
+ , velocity(mean.velocity + error.template segment<3>(9))
+ {
+ assert(!has_nan());
+ }
+
+#if 0
+ template <typename Vector_T>
+ state(const state& mean, const Vector_T& error, bool)
+ : gyro_bias(mean.gyro_bias + error.segment(0, 3))
+ , orientation(mean.orientation *
exp<double>(error.segment(3, 3)))
+ , position(mean.position + error.segment(6, 3).template
cast<double>())
+ , velocity(mean.velocity + error.segment(9, 3))
+ {
+ assert(!has_nan());
+ }
+#endif
+ /**
+ * Default-construct an undefined state object.
+ * @return
+ */
+ state(){}
+
+ /**
+ * Provided a kalman update vector, apply the vector as an
offset to this
+ * state.
+ * @param update A 12-vector to be applied
+ * @return The rotation applied to the mean orientation
+ */
+ Quaterniond apply_kalman_vec_update(const Matrix<double, 12, 1>
update);
+ Quaterniond apply_left_kalman_vec_update(const Matrix<double,
12, 1> update);
+
+ /**
+ * An estimate of the bias error in the rate gyros, in
radians/second
+ */
+ Vector3d gyro_bias;
+
+ /**
+ * An estimate of the orientation of the vehicle. This
quaternion represents
+ * a transformation from ECEF coordinates to the vehicle body
frame.
+ */
+ Quaterniond orientation;
+
+ /// Position in Earth-centered Earth-fixed reference frame, in
meters
+ Vector3d position;
+
+ /// Velocity in Earth-centered Earth-fixed reference frame, in
m/s
+ Vector3d velocity;
+
+ /**
+ * @return True if the state vector contains any NaNs
+ */
+ bool has_nan(void) const;
+
+ /**
+ * @return True if the state vector does not contain any NaNs
or Infs
+ */
+ bool is_real(void) const;
+
+ /**
+ * Print a representation of this object to the stream str.
+ * @param str An output stream.
+ */
+ void print(std::ostream& str);
+ };
+
+ /// The average state of the filter at any time t.
+ state avg_state;
+
+ /// Covariance term. Elements are ordered exactly as in struct state
+ Matrix<double, 12, 12> cov;
+
+ /**
+ * Initialize a new basic INS QKF
+ * @param pos_estimate Initial estimate of the position
+ * @param pos_error one-sigma initial bounds for position error
+ * @param bias_error one-sigma initial bounds for bias error in the
gyros
+ * @param v_error one-sigma bounds for velocity error (initial v == 0)
+ * @param gyro_white_noise The diagonal matrix of gyro white noise
+ * @param gyro_stability_noise The diagonal matrix of gyro instability
noise
+ * @param accel_white_noise The diagonal matrix of accelerometer white
noise
+ */
+ basic_ins_qkf(const Vector3d& pos_estimate,
+ double pos_error, double bias_error, double v_error,
+ const Vector3d& gyro_white_noise,
+ const Vector3d& gyro_stability_noise,
+ const Vector3d& accel_white_noise,
+ const Vector3d& vel_estimate = Vector3d::Zero());
+
+ /**
+ * Report an INS observation, to propagate the filter forward by one
time
+ * step. The coordinate system is maintained in ECEF coordinates.
+ * TODO: Provide a workspace parameter for storage that should be
+ * carried forward to an observation function carried out in this
+ * time step.
+ * @param gyro_meas The measured angular velocity, in rad/sec
+ * @param accel_meas The measured inertial reference frame
acceleration, in m/s
+ * @param dt The elapsed time since the last measurement, in seconds.
+ */
+ void predict(const Vector3d& gyro_meas, const Vector3d& accel_meas,
double dt);
+
+ /**
+ * Report an INS observation, to propagate the filter forward by one
time
+ * step. This function differs from predict() in that it uses the NED
frame
+ * instead of the ECEF frame.
+ */
+ void predict_ned(const Vector3d& gyro_meas, const Vector3d& accel_meas,
double dt);
+
+ /**
+ * Make a single vector observation, with some angular uncertainty.
+ * Warning: only one of obs_vector or obs_gps_pv_report can be called
+ * after a single call to predict().
+ * @param ref Reference vector, when the orientation is the
identity.
+ * Must be a unit vector.
+ * @param obs Vector observation, should not be a unit vector
+ * @param error one-sigma squared magnitude error in the
observation
+ */
+ void obs_vector(const Vector3d& ref, const Vector3d& obs, double error);
+
+ /**
+ * Incorporate a GPS PVT report.
+ * @param pos The sensor position, in earth-centered, earth-fixed
coords, meters
+ * @param vel The sensor velocity, in meters/second
+ * @param p_error The RMS position error, (m)^2
+ * @param v_error The RMS velocity error, (m/s)^2
+ */
+ void obs_gps_pv_report(const Vector3d& pos, const Vector3d& vel, const
Vector3d& p_error, const Vector3d v_error);
+
+ /**
+ * Incorporate a GPS position report, in either ECEF or NED coordinates.
+ * @param pos The position, in meters
+ * @param p_error The position error, in meters.
+ */
+ void obs_gps_p_report(const Vector3d& pos, const Vector3d& p_error);
+
+ /**
+ * Incorporate a GPS velocity report, in ECEF 3d coordinates.
+ * @param vel The 3d velocity, relative to the fixed earth frame, in
(m/s).
+ * @param v_error The one-sigma RMS velocity error (m/s)^2
+ */
+ void obs_gps_v_report(const Vector3d& vel, const Vector3d& v_error);
+
+ /**
+ * Observe a GPS vector track over ground report, in north-east-down
coordinates
+ * @param vel The 2d velocity value, parallel to the ground, in m/s
+ * @param v_error The one-sigma RMS velocity error (m/s)^2
+ */
+ void obs_gps_vtg_report(const Vector2d vel, const double v_error);
+
+ /**
+ * Directly observe the gyro sensor bias. In practice, we cannot do
this. However,
+ * the true bias is not a random walk. It tends to return towards zero
when it is
+ * farther away from zero (not temperature dependent). Therefore, we can
+ * incorporate this extra knowledge through a periodic "observation" of
zero
+ * bias with a large error.
+ *
+ * Use with extreme caution. It is almost certainly better to just
clamp the
+ * bias term after making an observation.
+ *
+ * @param bias The observed bias, in radians/sec
+ * @param bias_error The one-sigma estimate of the gyro bias error, in
radians/sec
+ */
+ void obs_gyro_bias(const Vector3d& bias, const Vector3d& bias_error);
+
+
+ /**
+ * Measure the total angular error between the filter's attitude
estimate
+ * and some other orientation.
+ * @param orientation The attitude to compare against
+ * @return The angular difference between them, in radians
+ */
+ double angular_error(const Quaterniond& orientation) const;
+
+ /**
+ * Measure the total gyro bias error between the filter's estimate and
+ * some other bias
+ * @param gyro_bias The gyro bias vector to compare against
+ * @return The vector difference between them, in radians/second
+ */
+ double gyro_bias_error(const Vector3d& gyro_bias) const;
+
+ /**
+ * Determine the statistical distance between an example sample point
+ * and the distribution computed by the estimator.
+ */
+ double mahalanobis_distance(const state& sample) const;
+
+private:
+
+ /**
+ * The type of an error term between two state vectors.
+ */
+ typedef Eigen::Matrix<double, 12, 1> state_error_t;
+ /// Compute the error difference between a sigma point and the mean as:
point - mean
+ state_error_t sigma_point_difference(const state& mean, const state&
point) const;
+
+
+public:
+ /** Perform the posterior counter-rotation of the covariance matrix by
+ the update that gets applied to the estimated state.
+ */
+ void counter_rotate_cov(const Quaterniond& update);
+
+ /**
+ * Verify that the covariance and average state are niether NaN nor Inf
+ * @return True, iff no element in the covariance or mean are NaN or Inf
+ */
+ bool is_real(void) const;
+};
+
+#endif
Added: paparazzi3/trunk/sw/airborne/fms/libeknav/ins_qkf_observe_gps_pvt.cpp
===================================================================
--- paparazzi3/trunk/sw/airborne/fms/libeknav/ins_qkf_observe_gps_pvt.cpp
(rev 0)
+++ paparazzi3/trunk/sw/airborne/fms/libeknav/ins_qkf_observe_gps_pvt.cpp
2010-09-10 14:26:27 UTC (rev 5849)
@@ -0,0 +1,119 @@
+/*
+ * ins_qkf_observe_gps_pvt.cpp
+ *
+ * Created on: Sep 2, 2009
+ * Author: Jonathan Brandmeyer
+ *
+ * This file is part of libeknav.
+ *
+ * Libeknav is free software: you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation, version 3.
+ *
+ * Libeknav is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ * GNU General Public License for more details.
+
+ * You should have received a copy of the GNU General Public License
+ * along with libeknav. If not, see <http://www.gnu.org/licenses/>.
+ */
+
+#include "ins_qkf.hpp"
+#include "assertions.hpp"
+#include <Eigen/LU>
+#include "timer.hpp"
+
+#ifdef TIME_OPS
+#include <iostream>
+#endif
+
+using namespace Eigen;
+
+#define RANK_ONE_UPDATES
+
+void
+basic_ins_qkf::obs_gps_v_report(const Vector3d& vel, const Vector3d& v_error)
+{
+ Vector3d residual = vel - avg_state.velocity;
+ std::cout << "diff_v(" <<residual(0) << ", " << residual(1) << ", " <<
residual(2) <<")\n";
+
+#ifdef RANK_ONE_UPDATES
+ Matrix<double, 12, 1> update = Matrix<double, 12, 1>::Zero();
+ for (int i = 0; i < 3; ++i) {
+ double innovation_cov_inv = 1.0/(cov(9+i, 9+i) + v_error[i]);
+ Matrix<double, 12, 1> gain = cov.block<12, 1>(0, 9+i) *
innovation_cov_inv;
+ update += gain * (residual[i] - update[9+i]);
+ cov -= gain * cov.block<1, 12>(9+i, 0);
+ }
+#else
+ Matrix<double, 3, 3> innovation_cov = cov.block<3, 3>(9, 9);
+ innovation_cov += v_error.asDiagonal();
+ Matrix<double, 3, 12> kalman_gain_t;
+ innovation_cov.qr().solve(cov.block<3, 12>(9, 0), &kalman_gain_t);
+ cov.part<Eigen::SelfAdjoint>() -= cov.block<12, 3>(0, 9) *
kalman_gain_t; // .transpose() * cov.block<3, 12>(9, 0);
+ Matrix<double, 12, 1> update = kalman_gain_t.transpose() * residual;
+#endif
+ std::cout << "update(" << update.segment<6>(0).transpose()*180/M_PI <<
"\t" << update.segment<6>(6).transpose() << ")\n";
+
+ //update.segment<6>(0) = Matrix<double, 6, 1>::Zero(); // only for
debugging
+ std::cout << "update(" << update.segment<6>(0).transpose()*180/M_PI <<
"\t" << update.segment<6>(6).transpose() << ")\n";
+ Quaterniond rotor = avg_state.apply_kalman_vec_update(update);
+ counter_rotate_cov(rotor);
+ assert(is_real());
+}
+
+void
+basic_ins_qkf::obs_gps_pv_report(
+ const Vector3d& pos, const Vector3d& vel,
+ const Vector3d& p_error, const Vector3d v_error)
+{
+#ifdef TIME_OPS
+ timer clock;
+ clock.start();
+#endif
+#if 1
+ obs_gps_p_report(pos, p_error);
+ obs_gps_v_report(vel, v_error);
+#else
+
+ // The observation model is strictly linear here, so use the linear
+ // form of the kalman gain and update
+ Matrix<double, 6, 12> obs_matrix;
+ obs_matrix << Matrix<double, 6, 6>::Zero(), Matrix<double, 6,
6>::Identity();
+
+ Matrix<double, 6, 1> residual;
+ residual.segment<3>(0) = (pos - avg_state.position);
+ residual.segment<3>(3) = vel - avg_state.velocity;
+
+ Matrix<double, 6, 6> innovation_cov = cov.corner<6,
6>(Eigen::BottomRight);
+ //innovation_cov = obs_matrix * cov * obs_matrix.transpose();
+ innovation_cov.corner<3, 3>(Eigen::TopLeft) += p_error.asDiagonal();
+ innovation_cov.corner<3, 3>(Eigen::BottomRight) += v_error.asDiagonal();
+#if 1
+ // Perform matrix inverse by LU decomposition instead of cofactor
expansion.
+ // K = P*transpose(H)*inverse(S)
+ // K = P*transpose(transpose(transpose(H)*inverse(S)))
+ // K = P*transpose(transpose(inverse(S))*H)
+ // K = P*transpose(inverse(transpose(S))*H)
+ // S == transpose(S)
+ // K = P*transpose(inverse(S)*H)
+ // obs_matrx <- inverse(S)*H
+ Matrix<double, 6, 12> inv_s_h;
+ innovation_cov.qr().solve(obs_matrix, &inv_s_h);
+ Matrix<double, 12, 6> kalman_gain = cov * inv_s_h.transpose();
+#else
+ Matrix<double, 12, 6> kalman_gain = cov.block<12, 6>(0, 6) *
innovation_cov.inverse();
+#endif
+ Quaterniond rotor = avg_state.apply_kalman_vec_update(kalman_gain *
residual);
+ cov.part<Eigen::SelfAdjoint>() -= kalman_gain * obs_matrix * cov;
+ counter_rotate_cov(rotor);
+ assert(is_real());
+#endif
+
+#ifdef TIME_OPS
+ double time = clock.stop() * 1e6;
+ std::cout << "obs_gps_pvt time: " << time << "\n";
+#endif
+}
+
Added: paparazzi3/trunk/sw/airborne/fms/libeknav/ins_qkf_observe_vector.cpp
===================================================================
--- paparazzi3/trunk/sw/airborne/fms/libeknav/ins_qkf_observe_vector.cpp
(rev 0)
+++ paparazzi3/trunk/sw/airborne/fms/libeknav/ins_qkf_observe_vector.cpp
2010-09-10 14:26:27 UTC (rev 5849)
@@ -0,0 +1,192 @@
+/*
+ * ins_qkf_observe_vector.cpp
+ *
+ * Created on: Sep 2, 2009
+ * Author: Jonathan Brandmeyer
+
+ * This file is part of libeknav.
+ *
+ * Libeknav is free software: you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation, version 3.
+ *
+ * Libeknav is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ * GNU General Public License for more details.
+
+ * You should have received a copy of the GNU General Public License
+ * along with libeknav. If not, see <http://www.gnu.org/licenses/>.
+ */
+
+#include "ins_qkf.hpp"
+#include "assertions.hpp"
+#include "timer.hpp"
+
+#ifdef TIME_OPS
+#include <iostream>
+#endif
+
+using namespace Eigen;
+#define RANK_ONE_UPDATES
+
+void
+basic_ins_qkf::obs_gyro_bias(const Vector3d& bias, const Vector3d& bias_error)
+{
+ Matrix<double, 12, 3> kalman_gain = cov.block<12, 3>(0, 0)
+ * (cov.block<3, 3>(0, 0) +
bias_error.asDiagonal()).inverse();
+ cov -= kalman_gain * cov.block<3, 12>(0, 0);
+ Vector3d innovation = bias - avg_state.gyro_bias;
+
+ // Apply the Kalman gain to obtain the posterior state and error
estimates.
+ avg_state.apply_kalman_vec_update(kalman_gain * innovation);
+}
+
+void
+basic_ins_qkf::obs_vector(const Vector3d& ref, const Vector3d& obs, double
error)
+{
+#ifdef TIME_OPS
+ timer clock;
+ clock.start();
+#endif
+#define DEBUG_VECTOR_OBS 0
+ // Optimization opportunity: re-use sigma points from the state
prediction
+#if 0
+ std::vector<vector_obs_state, aligned_allocator<vector_obs_state> >
points;
+ Matrix<double, 15, 30> state_errors;
+ decompose_sigma_points(points, state_errors, error);
+
+ std::vector<Quaterniond> projected_points;
+ projected_points.reserve(points.size());
+ for (auto i = points.begin(), end = points.end(); i != end; ++i) {
+ projected_points.push_back(observe_vector(*i, ref));
+ }
+
+ Vector3d expected_obs = avg_state.orientation * ref;
+ Quaterniond expected_observation_prediction =
Quaterniond().setFromTwoVectors(ref, expected_obs);
+ // Compute the observation error matrix and its self-covariance
+ Quaterniond mean_observation_pred =
quaternion_avg_johnson(projected_points);
+
+
+ // assert(obs_projection.transpose().isUnitary());
+#if DEBUG_VECTOR_OBS
+ std::cout << "\n\nref: " << ref.transpose();
+ std::cout << "\nobs: " << obs.transpose();
+ std::cout << "\nexpected observation: " <<
expected_observation_prediction.coeffs().transpose();
+ std::cout << "\nUKF observation: " <<
mean_observation_pred.coeffs().transpose();
+ std::cout << "\nexpected innovation: " <<
log<double>(Quaterniond().setFromTwoVectors(
+ expected_obs, obs)).transpose();
+ std::cout << "\ntangent space innovation: " <<
log<double>(avg_state.orientation.conjugate() *
+ Quaterniond().setFromTwoVectors(expected_obs, obs) *
avg_state.orientation) << "\n";
+#endif
+ Matrix<double, 3, Dynamic> obs_errors(3, projected_points.size());
+ for (unsigned i = 0; i != projected_points.size(); ++i) {
+ if
(projected_points[i].coeffs().dot(mean_observation_pred.coeffs()) < 0) {
+ // Choose the point on the same hemisphere as the mean.
otherwise, the error vectors
+ // get screwed up.
+ projected_points[i].coeffs() *= -1;
+ }
+ obs_errors.col(i) =
log<double>(mean_observation_pred.conjugate() * projected_points[i]);
+ }
+
+ // Construct an observation matrix composed of the two unit vectors
+ // orthogonal to the direction that pivots about the expected
observation
+ // Vector3d obs = obs.normalized();
+ Matrix<double, 2, 3> obs_projection;
+#if 1
+ SelfAdjointEigenSolver<Matrix<double, 3, 3> > obs_cov((obs_errors *
obs_errors.transpose())*0.5);
+ obs_projection = obs_cov.eigenvectors().block<3, 2>(0, 1).transpose();
+#else
+ typedef Vector3d vector_t;
+ obs_projection.row(0) =
expected_obs.cross((expected_obs.dot(vector_t::UnitX()) < 0.707)
+ ? vector_t::UnitX() :
vector_t::UnitY()).normalized();
+ obs_projection.row(1) = expected_obs.cross(obs_projection.row(0));
+#endif
+ assert(!hasNaN(obs_projection));
+#if DEBUG_VECTOR_OBS
+ std::cout << "predicted obs: " << expected_obs.transpose() << "\n";
+ std::cout << "actual obs: " << obs.transpose() << "\n";
+ std::cout << "reference obs: " << ref.transpose() << "\n";
+ std::cout << "obs cov eigenvalues: " <<
obs_cov.eigenvalues().transpose() << "\n";
+ std::cout << "obs cov eigenvectors:\n" << obs_cov.eigenvectors() <<
"\n";
+ std::cout << "measurement subspace:\n" << obs_projection.transpose() <<
"\n";
+#endif
+
+ Matrix<double, 2, Dynamic> projected_obs_errors =
obs_projection*obs_errors;
+ Matrix<double, 2, 2> obs_pred_cov = (projected_obs_errors *
projected_obs_errors.transpose())*0.5;
+#if DEBUG_VECTOR_OBS
+ std::cout << "obs errors: " << obs_errors << "\n";
+ std::cout << "S: " << obs_pred_cov << "\n";
+ std::cout << "expected S: " << cov.block<3, 3>(3, 3) << "\n";
+#endif
+
+ Matrix<double, 12, 2> state_meas_cross_cov =
+ (state_errors.block<12, 30>(0,0) *
projected_obs_errors.transpose())*0.5;
+ Matrix<double, 12, 2> kalman_gain = state_meas_cross_cov *
obs_pred_cov.inverse();
+ // Innovation rotation in the tangent space at the mean
+ Vector2d innovation = obs_projection*log<double>(
+ Quaterniond().setFromTwoVectors((mean_observation_pred
* ref), obs));
+#if DEBUG_VECTOR_OBS
+ std::cout << "inverse(S): " << obs_pred_cov.inverse() << "\n";
+ std::cout << "used innovation: " <<
log<double>(Quaterniond().setFromTwoVectors(
+ mean_observation_pred * ref, obs)).transpose() << "\n";
+ std::cout << "projected innovation: " << innovation.transpose() << "\n";
+ std::cout << "deprojected innovation: " << (obs_projection.transpose()
* innovation).transpose() << "\n";
+#endif
+ // actually, cov -= K*S*K^T, but this saves a multiplication.
+ cov -= (kalman_gain * obs_pred_cov * kalman_gain.transpose());
+#else
+ // BIG optimization opportunity: Use a pseudo-linear measurement model.
+
+ Vector3d obs_ref = avg_state.orientation.conjugate()*obs;
+ Vector3d v_residual = log<double>(Quaterniond().setFromTwoVectors(ref,
obs_ref));
+ std::cout << "v_residual(" <<v_residual(0)*180/M_PI << ", " <<
v_residual(1)*180/M_PI << ", " << v_residual(2)*180/M_PI <<")\n";
+
+ Matrix<double, 3, 2> h_trans;
+ h_trans.col(0) = ref.cross(
+ (abs(ref.dot(obs_ref)) < 0.9994) ? obs_ref :
+ (abs(ref.dot(Vector3d::UnitX())) < 0.707)
+ ? Vector3d::UnitX() :
Vector3d::UnitY()).normalized();
+ h_trans.col(1) = -ref.cross(h_trans.col(0));
+ assert(!hasNaN(h_trans));
+ assert(h_trans.isUnitary());
+ Vector2d innovation = h_trans.transpose() * v_residual;
+
+#ifdef RANK_ONE_UPDATES
+ // Running a rank-one update here is a strict win.
+ Matrix<double, 12, 1> update = Matrix<double, 12, 1>::Zero();
+ for (int i = 0; i < 2; ++i) {
+ double obs_error = error;
+ double obs_cov = (h_trans.col(i).transpose() * cov.block<3,
3>(3, 3) * h_trans.col(i))[0];
+ Matrix<double, 12, 1> gain = cov.block<12, 3>(0, 3) *
h_trans.col(i) / (obs_error + obs_cov);
+ update += gain * h_trans.col(i).transpose() * v_residual;
+ cov -= gain * h_trans.col(i).transpose() * cov.block<3, 12>(3,
0);
+ }
+#else
+ Matrix<double, 12, 2> kalman_gain = cov.block<12, 3>(0, 3) * h_trans
+ * (h_trans.transpose() * cov.block<3, 3>(3, 3) *
h_trans
+ + (Vector2d() << error,
error).finished().asDiagonal()).inverse();
+ cov -= kalman_gain * h_trans.transpose() * cov.block<3, 12>(3, 0);
+#endif
+
+#endif
+
+ // Apply the Kalman gain to obtain the posterior state and error
estimates.
+#ifndef RANK_ONE_UPDATES
+ Matrix<double, 12, 1> update = (kalman_gain * innovation);
+#endif
+
+#if DEBUG_VECTOR_OBS
+ // std::cout << "projected update: " << (obs_projection *
update.segment<3>(3)).transpose() << "\n";
+ std::cout << "deprojected update: " << update.segment<3>(3).transpose()
<< "\n";
+#endif
+ Quaterniond posterior_update =
avg_state.apply_kalman_vec_update(update);
+ counter_rotate_cov(posterior_update);
+
+ assert(is_real());
+#ifdef TIME_OPS
+ double time = clock.stop() * 1e6;
+ std::cout << "observe_vector(): " << time << "\n";
+#endif
+
+}
Added: paparazzi3/trunk/sw/airborne/fms/libeknav/ins_qkf_predict.cpp
===================================================================
--- paparazzi3/trunk/sw/airborne/fms/libeknav/ins_qkf_predict.cpp
(rev 0)
+++ paparazzi3/trunk/sw/airborne/fms/libeknav/ins_qkf_predict.cpp
2010-09-10 14:26:27 UTC (rev 5849)
@@ -0,0 +1,164 @@
+/*
+ * ins_qkf_predict.cpp
+ *
+ * Created on: Sep 2, 2009
+ * Author: Jonathan Brandmeyer
+
+ * This file is part of libeknav.
+ *
+ * Libeknav is free software: you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation, version 3.
+ *
+ * Libeknav is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ * GNU General Public License for more details.
+
+ * You should have received a copy of the GNU General Public License
+ * along with libeknav. If not, see <http://www.gnu.org/licenses/>.
+ */
+
+#include "ins_qkf.hpp"
+#include "assertions.hpp"
+#include "timer.hpp"
+
+#ifdef TIME_OPS
+# include <iostream>
+#endif
+
+using namespace Eigen;
+
+namespace {
+
+Matrix<double, 3, 3>
+axis_scale(const Vector3d& axis, double scale)
+{
+ return (scale - 1) * axis * axis.transpose() + Matrix3d::Identity();
+}
+
+void
+linear_predict(basic_ins_qkf& _this, const Vector3d& gyro_meas, const
Vector3d& accel_meas, double dt)
+{
+ // The two components of rotation that do not spin about the gravity
vector
+ // have an influence on the position and velocity of the vehicle.
+ // Let r be an error axis of rotation, and z be the gravity vector.
+ // Increasing r creates increasing error in the direction _|_ to r and
z.
+ // By the small angle theorem, the amount of error is ~ abs(r)*abs(z).
+ // Increasing r also creates increasing error in the direction || to -z.
+ // By the small angle theorem, the amount of error is ~ zero.
+ // Therefore, rotate the error block about the z axis by -90 degrees,
and
+ // zero out the error vector in the z direction.
+ // accel_cov is the relationship between error vectors in the tangent
space
+ // of the vehicle orientation and the translational reference frame.
+ Vector3d accel_body =
_this.avg_state.orientation.conjugate()*accel_meas; // accel_body
is in ECEF!
+ std::cout << " ACCEL_BODY(" << accel_body.transpose() << ")\n";
+ Vector3d accel_gravity = _this.avg_state.position.normalized()*9.81;
+ std::cout << " ACCEL_GRAVITY(" << accel_gravity.transpose() << ")\n";
+ //Vector3d accel_resid = accel_body - accel_gravity;
+ Vector3d accel_resid = accel_body + accel_gravity;
// This is better!
+ std::cout << " ACCEL_RESID(" << accel_resid.transpose() << ")\n";
+#if 0
+ // This form works well with zero static acceleration.
+ Matrix<double, 3, 3> accel_cov =
+ Eigen::AngleAxisd(-M_PI*0.5,
_this.avg_state.position.normalized())
+ * axis_scale(_this.avg_state.position.normalized(), 0) * 9.81;
+#elif 1
+ Matrix<double, 3, 3> accel_cov =
+ Eigen::AngleAxisd(-M_PI*0.5, accel_body.normalized())
+ * axis_scale(accel_body.normalized(), 0) * accel_meas.norm();
+#else
+ // The following form ends up being identical to the simpler one
+ // above
+ Matrix<double, 3, 3> accel_cov =
+ Eigen::AngleAxisd(-M_PI*0.5,
_this.avg_state.position.normalized())
+ * axis_scale(_this.avg_state.position.normalized(), 0) * 9.81
+ + Eigen::AngleAxisd(-M_PI*0.5, accel_resid.normalized())
+ * axis_scale(accel_resid.normalized(), 0)*accel_resid.norm();
+#endif
+ // TODO: Optimization opportunity: the accel_cov doesn't change much
over
+ // the life of a mission. Precompute it once and then retain the
original.
+ // Then, only one 3x3 block ever gets updated in the A matrix below.
+
+ // The linearized Kalman state projection matrix.
+#if 0
+ Matrix<double, 12, 12> A;
+ // gyro bias row
+ A << Matrix<double, 3, 3>::Identity(), Matrix<double, 3, 9>::Zero(),
+ // Orientation row
+ _this.avg_state.orientation.conjugate().toRotationMatrix()*-dt,
+ Matrix<double, 3, 3>::Identity(), Matrix<double, 3,
6>::Zero(),
+ // Position row
+ Matrix<double, 3, 3>::Zero(), -accel_cov*0.5*dt*dt,
+ Matrix<double, 3, 3>::Identity(), Matrix<double, 3,
3>::Identity()*dt,
+ // Velocity row
+ Matrix<double, 3, 3>::Zero(), -accel_cov * dt,
+ Matrix<double, 3, 3>::Zero(), Matrix<double, 3,
3>::Identity();
+
+ // 800x realtime, with vectorization
+ _this.cov.part<Eigen::SelfAdjoint>() = A * _this.cov * A.transpose();
+#else
+ // 1500x realtime, without vectorization, on 2.2 GHz Athlon X2
+ const Matrix<double, 12, 12> cov = _this.cov;
+ const Matrix3d dtR = dt *
_this.avg_state.orientation.conjugate().toRotationMatrix();
+ const Matrix3d dtQ = accel_cov * dt;
+
+ _this.cov.block<3, 3>(0, 3) -= cov.block<3,3>(0, 0)*dtR.transpose();
+ _this.cov.block<3, 3>(0, 6) += dt * cov.block<3, 3>(0, 9);
+ _this.cov.block<3, 3>(0, 9) -= cov.block<3, 3>(0, 3) * dtQ.transpose();
+ _this.cov.block<3, 3>(3, 3).part<Eigen::SelfAdjoint>() +=
dtR*cov.block<3, 3>(0, 0)*dtR.transpose()
+ - dtR*cov.block<3, 3>(0, 3) - cov.block<3, 3>(3,
0)*dtR.transpose();
+ _this.cov.block<3, 3>(3, 6) += -dtR * (cov.block<3, 3>(0, 6) +
dt*cov.block<3, 3>(0, 9))
+ + dt*cov.block<3, 3>(3, 9);
+ _this.cov.block<3, 3>(3, 9) += -dtR*( -cov.block<3, 3>(0,
3)*dtQ.transpose() + cov.block<3, 3>(0, 9))
+ - cov.block<3, 3>(3, 3)*dtQ.transpose();
+ _this.cov.block<3, 3>(6, 6).part<Eigen::SelfAdjoint>() +=
dt*cov.block<3, 3>(6, 9) + dt*dt*cov.block<3, 3>(9, 9)
+ + dt*cov.block<3, 3>(9, 6);
+ _this.cov.block<3, 3>(6, 9) += -cov.block<3, 3>(6, 3)*dtQ.transpose() +
dt*cov.block<3, 3>(9, 9)
+ - dt*cov.block<3, 3>(9, 3)*dtQ.transpose();
+ _this.cov.block<3, 3>(9, 9).part<Eigen::SelfAdjoint>() +=
dtQ*cov.block<3, 3>(3, 3)*dtQ.transpose()
+ - dtQ*cov.block<3, 3>(3, 9) - cov.block<3, 3>(9,
3)*dtQ.transpose();
+
+ _this.cov.block<3, 3>(3, 0) = _this.cov.block<3, 3>(0, 3).transpose();
+ _this.cov.block<3, 3>(6, 0) = _this.cov.block<3, 3>(0, 6).transpose();
+ _this.cov.block<3, 3>(6, 3) = _this.cov.block<3, 3>(3, 6).transpose();
+ _this.cov.block<3, 3>(9, 0) = _this.cov.block<3, 3>(0, 9).transpose();
+ _this.cov.block<3, 3>(9, 3) = _this.cov.block<3, 3>(3, 9).transpose();
+ _this.cov.block<3, 3>(9, 6) = _this.cov.block<3, 3>(6, 9).transpose();
+#endif
+
+ _this.cov.block<3, 3>(0, 0) += _this.gyro_stability_noise.asDiagonal()
* dt;
+ _this.cov.block<3, 3>(3, 3) += _this.gyro_white_noise.asDiagonal() * dt;
+ _this.cov.block<3, 3>(6, 6) += _this.accel_white_noise.asDiagonal() *
0.5*dt*dt;
+ _this.cov.block<3, 3>(9, 9) += _this.accel_white_noise.asDiagonal() *
dt;
+
+ Quaterniond orientation = exp<double>((gyro_meas -
_this.avg_state.gyro_bias) * dt)
+ * _this.avg_state.orientation;
+ Vector3d accel = accel_body - _this.avg_state.position.normalized() *
9.81;
+ Vector3d position = _this.avg_state.position + _this.avg_state.velocity
* dt + 0.5*accel*dt*dt;
+ Vector3d velocity = _this.avg_state.velocity + accel*dt;
+
+ _this.avg_state.position = position;
+ _this.avg_state.velocity = velocity;
+ _this.avg_state.orientation = orientation;
+}
+
+} // !namespace (anon)
+
+void
+basic_ins_qkf::predict(const Vector3d& gyro_meas, const Vector3d& accel_meas,
double dt)
+{
+#ifdef TIME_OPS
+ timer clock;
+ clock.start();
+#endif
+
+ // Always use linearized prediction
+ linear_predict(*this, gyro_meas, accel_meas, dt);
+
+#ifdef TIME_OPS
+ double time = clock.stop()*1e6;
+ std::cout << "unscented predict time: " << time << "\n";
+#endif
+}
+
Added: paparazzi3/trunk/sw/airborne/fms/libeknav/quaternions.hpp
===================================================================
--- paparazzi3/trunk/sw/airborne/fms/libeknav/quaternions.hpp
(rev 0)
+++ paparazzi3/trunk/sw/airborne/fms/libeknav/quaternions.hpp 2010-09-10
14:26:27 UTC (rev 5849)
@@ -0,0 +1,143 @@
+#ifndef AHRS_QUATERNIONS_HPP
+#define AHRS_QUATERNIONS_HPP
+
+/*
+ * quaternions.cpp
+ *
+ * Author: Jonathan Brandmeyer
+ *
+ * This file is part of libeknav.
+ *
+ * Libeknav is free software: you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation, version 3.
+ *
+ * Libeknav is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ * GNU General Public License for more details.
+
+ * You should have received a copy of the GNU General Public License
+ * along with libeknav. If not, see <http://www.gnu.org/licenses/>.
+ */
+
+#include <Eigen/Core>
+#include <Eigen/Geometry>
+
+using Eigen::Quaternion;
+
+template<typename FloatT>
+Quaternion<FloatT> operator-(const Quaternion<FloatT>& q)
+{
+ return Quaternion<FloatT>(-q.w(), -q.x(), -q.y(), -q.z());
+}
+
+/**
+ * Convert a rotation from modified Rodrigues parameters to a quaternion.
+ * @param v The multiplication of the rotation angle by the tangent of the
angle/4
+ * @return The quaternion corresponding to that rotation.
+ */
+template<typename FloatT>
+Quaternion<FloatT> exp_r(const Eigen::Matrix<FloatT, 3, 1>& v)
+{
+ // a2 = tan^2(theta/4)
+ FloatT a2 = v.squaredNorm();
+ Quaternion<FloatT> ret;
+ // sin(theta/2) = 2*tan(theta/4) / (1 + tan^2(theta/4))
+ // v == v.normalized() * tan^2(theta/4)
+ ret.vec() = v*(2/(1+a2));
+ // cos(theta/2) = (1 - tan^2(theta/4)) / (1 + tan^2(theta/4))
+ ret.w() = (1-a2)/(1+a2);
+ return ret;
+}
+
+template<typename FloatT>
+Eigen::Matrix<FloatT, 3, 1> log_r(const Quaternion<FloatT>& q)
+{
+ // Note: This algorithm is reasonably safe when using double
+ // precision (to within 1e-10 of precision), but not float.
+ /*
+ * q.w() == cos(theta/2)
+ * q.vec() == sin(theta/2)*v_hat
+ *
+ *
+ * Normal rodrigues params:
+ * v*tan(theta/2) = v*sin(theta/2) / cos(theta/2)
+ * v*tan(theta/2) = q.vec() / q.w()
+ *
+ * Modified rodrigues params (pulls away from infinity at theta=pi)
+ * tan(theta/2) == sin(theta) / (1 + cos(theta))
+ * therefore, tan(theta/4)*v_hat = sin(theta/2)*v_hat / (1 +
cos(theta/2))
+ */
+ return q.vec() / (1.0+q.w());
+}
+
+/**
+ * Convert an angle/axis 3-vector to a unit quaternion
+ * @param v A 3-vector whose length is between 0 and 2*pi
+ * @return The quaternion that represents the same rotation.
+ */
+template<typename FloatT>
+Quaternion<FloatT> exp(Eigen::Matrix<FloatT, 3, 1> v);
+
+template<typename FloatT>
+Quaternion<FloatT> exp(Eigen::Matrix<FloatT, 3, 1> v)
+{
+ FloatT angle = v.norm();
+ if (angle <= Eigen::machine_epsilon<FloatT>()) {
+ // std::cerr << "Warning: tiny quaternion flushed to zero\n";
+ return Quaternion<FloatT>::Identity();
+ }
+ else {
+ Quaternion<FloatT> ret;
+#if 0
+ if (angle > 1.999*M_PI) {
+ // TODO: I really, really don't like this hack. It
should
+ // be impossible to compute an angular measurement
update
+ // with a rotation angle greater than this number...
+ v *= 1.999*M_PI / angle;
+ angle = 1.999*M_PI;
+ }
+#endif
+ assert(angle <= FloatT(2.0*M_PI));
+#if 0
+ // Oddly enough, this attempt to make the formula faster by
reducing
+ // the number of trig calls actually runs slower.
+ FloatT tan_x = std::tan(angle * 0.25);
+ FloatT cos_angle = (1 - tan_x*tan_x)/(1+tan_x*tan_x);
+ FloatT sin_angle = 2*tan_x/(1+tan_x*tan_x);
+ ret.w() = cos_angle;
+ ret.vec() = (sin_angle/angle)*v;
+#else
+ ret.w() = std::cos(angle*0.5);
+ ret.vec() = (std::sin(angle*0.5)/angle)*v;
+#endif
+ return ret;
+ // return Quaternion<FloatT>(Eigen::AngleAxis<FloatT>(angle, v
/ angle));
+ }
+}
+
+/**
+ * Convert a unit quaternion to multiplied angle/axis form.
+ * @param q A unit quaternion. The quaternion's norm should be close to unity,
+ * but may be slightly too large or small.
+ * @return The 3-vector in the tangent space of the quaternion q.
+ */
+template<typename FloatT>
+Eigen::Matrix<FloatT, 3, 1> log(const Quaternion<FloatT>& q)
__attribute__((noinline));
+
+template<typename FloatT>
+Eigen::Matrix<FloatT, 3, 1> log(const Quaternion<FloatT>& q)
+{
+ FloatT mag = q.vec().norm();
+ if (mag <= Eigen::machine_epsilon<FloatT>()) {
+ // Flush to zero for very small angles. This avoids division
by zero.
+ return Eigen::Matrix<FloatT, 3, 1>::Zero();
+ }
+ FloatT angle = 2.0*std::atan2(mag, q.w());
+ return q.vec() * (angle/mag);
+ // Eigen::AngleAxis<FloatT> res(q /*mag <= 1.0) ? q : q.normalized()
*/);
+ // return res.axis() * res.angle();
+}
+
+#endif
| [Prev in Thread] |
Current Thread |
[Next in Thread] |
- [paparazzi-commits] [5849] added libeknav,
antoine drouin <=