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 25 Packages Projects Releases Wiki Activity

orientation is now initilaizable

Browse Source
This commit is contained in:
Martin Dieblich
2010-09-24 08:44:42 +00:00
parent 841cd14bbb
commit 6252c082e6
3 changed files with 6 additions and 4 deletions
Download Patch File Download Diff File Expand all files Collapse all files
sw/airborne/fms/libeknav/basic_ins_qkf.cpp
+3 -1
View File
@@ -30,6 +30,7 @@ basic_ins_qkf::basic_ins_qkf(
const Vector3d& gyro_white_noise,
const Vector3d& gyro_stability_noise,
const Vector3d& accel_white_noise,
Quaterniond initial_orientation, // this is new
const Vector3d& vel_estimate)
: gyro_stability_noise(gyro_stability_noise)
, gyro_white_noise(gyro_white_noise)
@@ -37,7 +38,8 @@ basic_ins_qkf::basic_ins_qkf(
{
avg_state.position = estimate;
avg_state.gyro_bias = Vector3d::Zero();
avg_state.orientation = Quaterniond::Identity();
//avg_state.orientation = Quaterniond::Identity();
avg_state.orientation = initial_orientation;
avg_state.velocity = vel_estimate;
cov << Matrix3d::Identity()*bias_error*bias_error, Matrix<double, 3, 9>::Zero(),
sw/airborne/fms/libeknav/ins_qkf.hpp
+1 -1
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>
sw/airborne/fms/libeknav/test_libeknav_2.cpp
+2 -2
View File
@@ -20,7 +20,7 @@ int main(int, char *[]) {
/* initial state */
Vector3d pos_0_ecef(1017.67e3, -5079.282e3, 3709.041e3);
Vector3d speed_0_ecef(0., 0., 0.);
// Vector3d orientation(0., 0., 0.);
Quaterniond orientation(1., 0., 0., 0.);
Vector3d bias_0(0., 0., 0.);
/* initial covariance */
@@ -45,7 +45,7 @@ int main(int, char *[]) {
Vector3d magnetometer = Vector3d::UnitZ();
basic_ins_qkf ins(pos_0_ecef, pos_cov_0, bias_cov_0, speed_cov_0,
gyro_white_noise, gyro_stability_noise, accel_white_noise);
gyro_white_noise, gyro_stability_noise, accel_white_noise,orientation);
const double dt = 1./512.; /* predict at 512Hz */
for (int i=1; i<5000; i++) {