rooty/LibrePilot
1
0
Fork 0
mirror of https://bitbucket.org/librepilot/librepilot.git synced 2024-12-01 09:24:10 +01:00
Code Issues Releases Activity

flight/AHRS: Make the 13 state INS default again, the accel bias drift can be a

Browse Source 
little aggressive.  Also increased the drift rate of the gyro bias per Sambas'
testing (thanks) although not to the levels required for flying through 40 deg
C transitions.

Made the dump_ekf function handle both numbers of states

git-svn-id: svn://svn.openpilot.org/OpenPilot/trunk@2282 ebee16cc-31ac-478f-84a7-5cbb03baadba
This commit is contained in:
peabody124 2010-12-24 19:38:54 +00:00 committed by peabody124
parent d96a9aace9
commit 314a2fcf00
6 changed files with 28 additions and 107 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

2
flight/AHRS/Makefile
View File

@ -102,7 +102,7 @@ SRC += $(FLIGHTLIB)/ahrs_comm_objects.c
SRC += $(BOOT)/ahrs_spi_program_slave.c
SRC += $(BOOT)/ahrs_slave_test.c
SRC += $(BOOT)/ahrs_spi_program.c
SRC += insgps.c
SRC += insgps13state.c
SRC += $(FLIGHTLIB)/CoordinateConversions.c
SRC += $(FLIGHTLIB)/fifo_buffer.c

18
flight/AHRS/ahrs.c
View File

@ -365,16 +365,12 @@ void simple_update() {
* @brief Output all the important inputs and states of the ekf through serial port
*/
#ifdef DUMP_EKF
#define NUMX 16 // number of states, X is the state vector
#define NUMW 12 // number of plant noise inputs, w is disturbance noise vector
#define NUMV 10 // number of measurements, v is the measurement noise vector
#define NUMU 7 // number of deterministic inputs, U is the input vector
extern float F[NUMX][NUMX], G[NUMX][NUMW], H[NUMV][NUMX]; // linearized system matrices
extern float P[NUMX][NUMX], X[NUMX]; // covariance matrix and state vector
extern float Q[NUMW], R[NUMV]; // input noise and measurement noise variances
extern float K[NUMX][NUMV]; // feedback gain matrix
extern float **P, *X; // covariance matrix and state vector
void print_ekf_binary()
{
uint16_t states = ins_get_num_states();
uint8_t framing[16] = { 15, 14, 13, 12, 11, 10, 9, 8, 7, 6, 5, 4, 3, 2, 1, 0 };
// Dump raw buffer
PIOS_COM_SendBuffer(PIOS_COM_AUX, &framing[0], 16); // framing header (1:16)
@ -388,9 +384,9 @@ void print_ekf_binary()
PIOS_COM_SendBuffer(PIOS_COM_AUX, (uint8_t *) & gps_data, sizeof(gps_data)); // gps data (58:85)
PIOS_COM_SendBuffer(PIOS_COM_AUX, (uint8_t *) & X, 4 * NUMX); // X (86:149)
for(uint8_t i = 0; i < NUMX; i++)
PIOS_COM_SendBuffer(PIOS_COM_AUX, (uint8_t *) &(P[i][i]), 4); // diag(P) (150:213)
PIOS_COM_SendBuffer(PIOS_COM_AUX, (uint8_t *) & X, 4 * states); // X (86:149)
for(uint8_t i = 0; i < states; i++)
PIOS_COM_SendBuffer(PIOS_COM_AUX, (uint8_t *) &((*P)[i + i * states]), 4); // diag(P) (150:213)
PIOS_COM_SendBuffer(PIOS_COM_AUX, (uint8_t *) & altitude_data.altitude, 4); // BaroAlt (214:217)
PIOS_COM_SendBuffer(PIOS_COM_AUX, (uint8_t *) & baro_offset, 4); // baro_offset (218:221)

2
flight/AHRS/inc/insgps.h
View File

@ -74,6 +74,8 @@ void GpsBaroCorrection(float Pos[3], float Vel[3], float BaroAlt);
void GpsMagCorrection(float mag_data[3], float Pos[3], float Vel[2]);
void VelBaroCorrection(float Vel[3], float BaroAlt);
uint16_t ins_get_num_states();
// Nav structure containing current solution
struct NavStruct {
float Pos[3]; // Position in meters and relative to a local NED frame

90
flight/AHRS/inc/insgps13state.h
View File

@ -1,90 +0,0 @@
/**
******************************************************************************
* @addtogroup AHRS
* @{
* @addtogroup INSGPS
* @{
* @brief INSGPS is a joint attitude and position estimation EKF
*
* @file insgps.h
* @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010.
* @brief Include file of the INSGPS exposed functionality.
*
* @see The GNU Public License (GPL) Version 3
*
*****************************************************************************/
/*
* This program 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; either version 3 of the License, or
* (at your option) any later version.
*
* This program 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 this program; if not, write to the Free Software Foundation, Inc.,
* 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
*/
#ifndef INSGPS_H_
#define INSGPS_H_
#include "stdint.h"
/**
* @addtogroup Constants
* @{
*/
#define POS_SENSORS 0x007
#define HORIZ_SENSORS 0x018
#define VERT_SENSORS 0x020
#define MAG_SENSORS 0x1C0
#define BARO_SENSOR 0x200
#define FULL_SENSORS 0x3FF
/**
* @}
*/
// Exposed Function Prototypes
void INSGPSInit();
void INSStatePrediction(float gyro_data[3], float accel_data[3], float dT);
void INSCovariancePrediction(float dT);
void INSCorrection(float mag_data[3], float Pos[3], float Vel[3], float BaroAlt, uint16_t SensorsUsed);
void INSResetP(float PDiag[13]);
void INSSetState(float pos[3], float vel[3], float q[4], float gyro_bias[3]);
void INSSetPosVelVar(float PosVar, float VelVar);
void INSSetGyroBias(float gyro_bias[3]);
void INSSetAccelVar(float accel_var[3]);
void INSSetGyroVar(float gyro_var[3]);
void INSSetMagNorth(float B[3]);
void INSSetMagVar(float scaled_mag_var[3]);
void INSPosVelReset(float pos[3], float vel[3]);
void MagCorrection(float mag_data[3]);
void MagVelBaroCorrection(float mag_data[3], float Vel[3], float BaroAlt);
void FullCorrection(float mag_data[3], float Pos[3], float Vel[3],
float BaroAlt);
void GpsBaroCorrection(float Pos[3], float Vel[3], float BaroAlt);
void GpsMagCorrection(float mag_data[3], float Pos[3], float Vel[2]);
void VelBaroCorrection(float Vel[3], float BaroAlt);
// Nav structure containing current solution
struct NavStruct {
float Pos[3]; // Position in meters and relative to a local NED frame
float Vel[3]; // Velocity in meters and in NED
float q[4]; // unit quaternion rotation relative to NED
float gyro_bias[3];
} Nav;
/**
* @}
* @}
*/
#endif /* EKF_H_ */

13
flight/AHRS/insgps13state.c
View File

@ -29,7 +29,7 @@
* 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
*/
#include "insgps13state.h"
#include "insgps.h"
#include <math.h>
#include <stdint.h>
@ -68,6 +68,12 @@ float K[NUMX][NUMV]; // feedback gain matrix
// ************* Exposed Functions ****************
// *************************************************
uint16_t ins_get_num_states()
{
return NUMX;
}
void INSGPSInit() //pretty much just a place holder for now
{
Be[0] = 1;
@ -92,7 +98,7 @@ void INSGPSInit() //pretty much just a place holder for now
Q[0] = Q[1] = Q[2] = 50e-8; // gyro noise variance (rad/s)^2
Q[3] = Q[4] = Q[5] = 0.01; // accelerometer noise variance (m/s^2)^2
Q[6] = Q[7] = Q[8] = 2e-14; // gyro bias random walk variance (rad/s^2)^2
Q[6] = Q[7] = Q[8] = 2e-9; // gyro bias random walk variance (rad/s^2)^2
R[0] = R[1] = 0.004; // High freq GPS horizontal position noise variance (m^2)
R[2] = 0.036; // High freq GPS vertical position noise variance (m^2)
@ -116,8 +122,9 @@ void INSResetP(float PDiag[NUMX])
}
}
void INSSetState(float pos[3], float vel[3], float q[4], float gyro_bias[3])
void INSSetState(float pos[3], float vel[3], float q[4], float gyro_bias[3], float accel_bias[3])
{
/* Note: accel_bias not used in 13 state INS */
X[0] = pos[0];
X[1] = pos[1];
X[2] = pos[2];

10
flight/AHRS/insgps.c → flight/AHRS/insgps16state.c
View File

@ -68,6 +68,12 @@ float K[NUMX][NUMV]; // feedback gain matrix
// ************* Exposed Functions ****************
// *************************************************
uint16_t ins_get_num_states()
{
return NUMX;
}
void INSGPSInit() //pretty much just a place holder for now
{
Be[0] = 1;
@ -92,8 +98,8 @@ void INSGPSInit() //pretty much just a place holder for now
Q[0] = Q[1] = Q[2] = 50e-8; // gyro noise variance (rad/s)^2
Q[3] = Q[4] = Q[5] = 0.01; // accelerometer noise variance (m/s^2)^2
Q[6] = Q[7] = Q[8] = 2e-14; // gyro bias random walk variance (rad/s^2)^2
Q[9] = Q[10] = Q[11] = 2e-13; // accel bias random walk variance (m/s^3)^2
Q[6] = Q[7] = Q[8] = 2e-9; // gyro bias random walk variance (rad/s^2)^2
Q[9] = Q[10] = Q[11] = 2e-20; // accel bias random walk variance (m/s^3)^2
R[0] = R[1] = 0.004; // High freq GPS horizontal position noise variance (m^2)
R[2] = 0.036; // High freq GPS vertical position noise variance (m^2)