Mirrors_Framework/paparazzi
1
0
Fork 0
mirror of https://github.com/paparazzi/paparazzi.git synced 2026-06-06 07:53:43 +08:00
Code Issues Actions 26 Packages Projects Releases Wiki Activity

ins_qkf: added init for the orientation

Browse Source 
predict:	changed some names
This commit is contained in:
Martin Dieblich
2010-09-24 08:23:11 +00:00
parent e132f25e4f
commit 841cd14bbb
2 changed files with 34 additions and 17 deletions
Download Patch File Download Diff File Expand all files Collapse all files
sw/airborne/fms/libeknav/ins_qkf.hpp
+12 -3
View File
@@ -20,7 +20,7 @@
* along with libeknav. If not, see <http://www.gnu.org/licenses/>.
*/
//#include "sigma_points.hpp"
#include "sigma_points.hpp"
#include "quaternions.hpp"
#include <Eigen/StdVector>
@@ -155,13 +155,23 @@ struct basic_ins_qkf
* @param gyro_stability_noise The diagonal matrix of gyro instability noise
* @param accel_white_noise The diagonal matrix of accelerometer white noise
*/
//Old one without orientation_init()
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,
Quaterniond initial_orientation = Quaterniond::Identity(),
const Vector3d& vel_estimate = Vector3d::Zero());
/* //Old one without orientation_init()
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.
@@ -237,7 +247,6 @@ struct basic_ins_qkf
*/
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.
sw/airborne/fms/libeknav/ins_qkf_predict.cpp
+22 -14
View File
@@ -19,11 +19,18 @@
* along with libeknav. If not, see <http://www.gnu.org/licenses/>.
*/
/**
* This is a modified version by Martin Dieblich
* The general changes are a more consequent naming
*
*/
#include "ins_qkf.hpp"
#include "assertions.hpp"
#include "timer.hpp"
#ifdef TIME_OPS
#include "timer.hpp"
# include <iostream>
#endif
@@ -51,13 +58,13 @@ linear_predict(basic_ins_qkf& _this, const Vector3d& gyro_meas, const Vector3d&
// 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";
Matrix3d rot = _this.avg_state.orientation.conjugate().toRotationMatrix();
Vector3d accel_ecef = rot*accel_meas; // a_e = (q_e2b)^* x a_b = q_b2e x a_b
Vector3d accel_gravity = _this.avg_state.position.normalized()*(-9.81);
Vector3d accel_resid = accel_ecef + accel_gravity; // a = (xdd-g)+g = xdd;
#if 0
// This form works well with zero static acceleration.
Matrix<double, 3, 3> accel_cov =
@@ -65,12 +72,12 @@ linear_predict(basic_ins_qkf& _this, const Vector3d& gyro_meas, const Vector3d&
* 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();
Eigen::AngleAxisd(-M_PI*0.5, accel_ecef.normalized())
* axis_scale(accel_ecef.normalized(), 0) * accel_meas.norm();
#else
// The following form ends up being identical to the simpler one
// above
Matrix<double, 3, 3> accel_cov =
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())
@@ -134,9 +141,10 @@ linear_predict(basic_ins_qkf& _this, const Vector3d& gyro_meas, const Vector3d&
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;
//Vector3d accel = accel_ecef - _this.avg_state.position.normalized() * 9.81;
//std::cout << " ACCEL______(" << accel.transpose() << ")\n";
Vector3d position = _this.avg_state.position + _this.avg_state.velocity * dt + 0.5*accel_resid*dt*dt;
Vector3d velocity = _this.avg_state.velocity + accel_resid*dt;
_this.avg_state.position = position;
_this.avg_state.velocity = velocity;