mirror of
https://github.com/paparazzi/paparazzi.git
synced 2026-06-05 23:49:00 +08:00
new horizontal filter based on vertical filter. define USE_HFF to use it
This commit is contained in:
Felix Ruess
@@ -204,6 +204,9 @@
|
||||
|
||||
include $(CFG_BOOZ)/subsystems/booz2_ahrs_cmpl.makefile
|
||||
|
||||
#enable horizontal filter
|
||||
ap.CFLAGS += -DUSE_HFF
|
||||
|
||||
</makefile>
|
||||
|
||||
</airframe>
|
||||
|
||||
@@ -130,7 +130,9 @@ ap.srcs += $(SRC_BOOZ)/guidance/booz2_guidance_v.c
|
||||
|
||||
ap.srcs += $(SRC_BOOZ)/booz2_ins.c
|
||||
ap.srcs += math/pprz_geodetic_int.c math/pprz_geodetic_float.c
|
||||
# horizontal filter float version
|
||||
ap.srcs += $(SRC_BOOZ)/ins/booz2_hf_float.c
|
||||
ap.CFLAGS += -DDT_HFILTER="(1./512)"
|
||||
# vertical filter float version
|
||||
ap.srcs += $(SRC_BOOZ)/ins/booz2_vf_float.c
|
||||
ap.CFLAGS += -DUSE_VFF -DDT_VFILTER="(1./512.)" -DFLOAT_T=float
|
||||
|
||||
@@ -29,13 +29,18 @@
|
||||
|
||||
#include "airframe.h"
|
||||
#include "math/pprz_algebra_int.h"
|
||||
#include "math/pprz_algebra_float.h"
|
||||
|
||||
#include "booz_ahrs.h"
|
||||
|
||||
#ifdef USE_VFF
|
||||
#include "ins/booz2_vf_float.h"
|
||||
#endif
|
||||
|
||||
#include "booz_ahrs.h"
|
||||
#ifdef USE_HFF
|
||||
#include "ins/booz2_hf_float.h"
|
||||
#endif
|
||||
|
||||
|
||||
#include "math/pprz_geodetic_int.h"
|
||||
|
||||
@@ -70,41 +75,59 @@ void booz_ins_init() {
|
||||
booz_ins_vff_realign = FALSE;
|
||||
b2_vff_init(0., 0., 0.);
|
||||
#endif
|
||||
//#ifdef SITL
|
||||
b2ins_init();
|
||||
//#endif
|
||||
#ifdef USE_HFF
|
||||
b2_hff_init(0., 0., 0., 0., 0., 0.);
|
||||
#endif
|
||||
INT32_VECT3_ZERO(booz_ins_ltp_pos);
|
||||
INT32_VECT3_ZERO(booz_ins_ltp_speed);
|
||||
INT32_VECT3_ZERO(booz_ins_ltp_accel);
|
||||
INT32_VECT3_ZERO(booz_ins_enu_pos);
|
||||
INT32_VECT3_ZERO(booz_ins_enu_speed);
|
||||
INT32_VECT3_ZERO(booz_ins_enu_accel);
|
||||
}
|
||||
|
||||
void booz_ins_propagate() {
|
||||
#ifdef BOOZ_INS_UNTILT_ACCEL
|
||||
struct Int32Vect3 accel_body;
|
||||
INT32_RMAT_TRANSP_VMULT(accel_body, booz_imu.body_to_imu_rmat, booz_imu.accel);
|
||||
struct Int32Vect3 accel_ltp;
|
||||
INT32_RMAT_TRANSP_VMULT(accel_ltp, booz_ahrs.ltp_to_body_rmat, accel_body);
|
||||
#ifdef USE_HFF
|
||||
float x_accel_float = ACCEL_FLOAT_OF_BFP(accel_ltp.x);
|
||||
float y_accel_float = ACCEL_FLOAT_OF_BFP(accel_ltp.y);
|
||||
#endif
|
||||
float z_accel_float = ACCEL_FLOAT_OF_BFP(accel_ltp.z);
|
||||
#else /* BOOZ_INS_UNTILT_ACCELS */
|
||||
#ifdef USE_HFF
|
||||
float x_accel_float = ACCEL_FLOAT_OF_BFP(booz_imu.accel.x);
|
||||
float y_accel_float = ACCEL_FLOAT_OF_BFP(booz_imu.accel.y);
|
||||
#endif
|
||||
float z_accel_float = ACCEL_FLOAT_OF_BFP(booz_imu.accel.z);
|
||||
#endif /* BOOZ_INS_UNTILT_ACCELS */
|
||||
|
||||
#ifdef USE_VFF
|
||||
if (booz2_analog_baro_status == BOOZ2_ANALOG_BARO_RUNNING && booz_ins_baro_initialised) {
|
||||
#ifdef BOOZ_INS_UNTILT_ACCEL
|
||||
struct Int32Vect3 accel_body;
|
||||
INT32_RMAT_TRANSP_VMULT(accel_body, booz_imu.body_to_imu_rmat, booz_imu.accel);
|
||||
struct Int32Vect3 accel_ltp;
|
||||
INT32_RMAT_TRANSP_VMULT(accel_ltp, booz_ahrs.ltp_to_body_rmat, accel_body);
|
||||
float accel_float = ACCEL_FLOAT_OF_BFP(accel_ltp.z);
|
||||
#else /* BOOZ_INS_UNTILT_ACCELS */
|
||||
float accel_float = ACCEL_FLOAT_OF_BFP(booz_imu.accel.z);
|
||||
#endif /* BOOZ_INS_UNTILT_ACCELS */
|
||||
b2_vff_propagate(accel_float);
|
||||
b2_vff_propagate(z_accel_float);
|
||||
booz_ins_ltp_accel.z = ACCEL_BFP_OF_REAL(b2_vff_zdotdot);
|
||||
booz_ins_ltp_speed.z = SPEED_BFP_OF_REAL(b2_vff_zdot);
|
||||
booz_ins_ltp_pos.z = POS_BFP_OF_REAL(b2_vff_z);
|
||||
booz_ins_enu_pos.z = -booz_ins_ltp_pos.z;
|
||||
booz_ins_enu_speed.z = -booz_ins_ltp_speed.z;
|
||||
booz_ins_enu_accel.z = -booz_ins_ltp_accel.z;
|
||||
}
|
||||
#endif
|
||||
#endif /* USE_VFF */
|
||||
|
||||
#ifdef USE_HFF
|
||||
if (booz_ahrs.status == BOOZ_AHRS_RUNNING &&
|
||||
booz_gps_state.fix == BOOZ2_GPS_FIX_3D && booz_ins_ltp_initialised )
|
||||
b2ins_propagate();
|
||||
#endif
|
||||
if (booz_ahrs.status == BOOZ_AHRS_RUNNING && booz_gps_state.fix == BOOZ2_GPS_FIX_3D && booz_ins_ltp_initialised ) {
|
||||
b2_hff_propagate(x_accel_float, y_accel_float);
|
||||
booz_ins_ltp_accel.x = ACCEL_BFP_OF_REAL(b2_hff_xdotdot);
|
||||
booz_ins_ltp_accel.y = ACCEL_BFP_OF_REAL(b2_hff_ydotdot);
|
||||
booz_ins_ltp_speed.x = SPEED_BFP_OF_REAL(b2_hff_xdot);
|
||||
booz_ins_ltp_speed.y = SPEED_BFP_OF_REAL(b2_hff_ydot);
|
||||
booz_ins_ltp_pos.x = POS_BFP_OF_REAL(b2_hff_x);
|
||||
booz_ins_ltp_pos.y = POS_BFP_OF_REAL(b2_hff_y);
|
||||
}
|
||||
#endif /* USE_HFF */
|
||||
INT32_VECT3_ENU_OF_NED(booz_ins_enu_pos, booz_ins_ltp_pos);
|
||||
INT32_VECT3_ENU_OF_NED(booz_ins_enu_speed, booz_ins_ltp_speed);
|
||||
INT32_VECT3_ENU_OF_NED(booz_ins_enu_accel, booz_ins_ltp_accel);
|
||||
}
|
||||
|
||||
void booz_ins_update_baro() {
|
||||
@@ -121,6 +144,12 @@ void booz_ins_update_baro() {
|
||||
booz_ins_vff_realign = FALSE;
|
||||
booz_ins_qfe = booz2_analog_baro_value;
|
||||
b2_vff_realign(0.);
|
||||
booz_ins_ltp_accel.z = ACCEL_BFP_OF_REAL(b2_vff_zdotdot);
|
||||
booz_ins_ltp_speed.z = SPEED_BFP_OF_REAL(b2_vff_zdot);
|
||||
booz_ins_ltp_pos.z = POS_BFP_OF_REAL(b2_vff_z);
|
||||
booz_ins_enu_pos.z = -booz_ins_ltp_pos.z;
|
||||
booz_ins_enu_speed.z = -booz_ins_ltp_speed.z;
|
||||
booz_ins_enu_accel.z = -booz_ins_ltp_accel.z;
|
||||
}
|
||||
b2_vff_update(alt_float);
|
||||
}
|
||||
@@ -138,17 +167,36 @@ void booz_ins_update_gps(void) {
|
||||
ned_of_ecef_point_i(&booz_ins_gps_pos_cm_ned, &booz_ins_ltp_def, &booz_gps_state.ecef_pos);
|
||||
ned_of_ecef_vect_i(&booz_ins_gps_speed_cm_s_ned, &booz_ins_ltp_def, &booz_gps_state.ecef_vel);
|
||||
#ifdef USE_HFF
|
||||
b2ins_update_gps();
|
||||
VECT2_SDIV(booz_ins_ltp_pos, b2ins_pos_ltp, (1<<(B2INS_POS_LTP_FRAC-INT32_POS_FRAC)));
|
||||
VECT2_SDIV(booz_ins_ltp_speed, b2ins_speed_ltp, (1<<(B2INS_SPEED_LTP_FRAC-INT32_SPEED_FRAC)));
|
||||
#else
|
||||
INT32_VECT3_SCALE_2(b2ins_meas_gps_pos_ned, booz_ins_gps_pos_cm_ned,
|
||||
INT32_POS_OF_CM_NUM, INT32_POS_OF_CM_DEN);
|
||||
INT32_VECT3_SCALE_2(b2ins_meas_gps_speed_ned, booz_ins_gps_speed_cm_s_ned,
|
||||
INT32_SPEED_OF_CM_S_NUM, INT32_SPEED_OF_CM_S_DEN);
|
||||
VECT3_COPY(booz_ins_ltp_pos, b2ins_meas_gps_pos_ned);
|
||||
VECT3_COPY(booz_ins_ltp_speed, b2ins_meas_gps_speed_ned);
|
||||
struct FloatVect2 gps_float;
|
||||
#ifdef B2_HFF_UPDATE_POS
|
||||
VECT2_ASSIGN(gps_float, booz_ins_gps_pos_cm_ned.x, booz_ins_gps_pos_cm_ned.y);
|
||||
VECT2_SDIV(gps_float, gps_float, 100.);
|
||||
b2_hff_update_pos(gps_float.x, gps_float.y);
|
||||
#endif
|
||||
#ifdef B2_HFF_UPDATE_SPEED
|
||||
VECT2_ASSIGN(gps_float, booz_ins_gps_speed_cm_s_ned.x, booz_ins_gps_speed_cm_s_ned.y);
|
||||
VECT2_SDIV(gps_float, gps_float, 100.);
|
||||
b2_hff_update_v(gps_float.x, gps_float.y);
|
||||
#endif
|
||||
booz_ins_ltp_accel.x = ACCEL_BFP_OF_REAL(b2_hff_xdotdot);
|
||||
booz_ins_ltp_accel.y = ACCEL_BFP_OF_REAL(b2_hff_ydotdot);
|
||||
booz_ins_ltp_speed.x = SPEED_BFP_OF_REAL(b2_hff_xdot);
|
||||
booz_ins_ltp_speed.y = SPEED_BFP_OF_REAL(b2_hff_ydot);
|
||||
booz_ins_ltp_pos.x = POS_BFP_OF_REAL(b2_hff_x);
|
||||
booz_ins_ltp_pos.y = POS_BFP_OF_REAL(b2_hff_y);
|
||||
#ifndef USE_VFF /* only hf */
|
||||
booz_ins_ltp_pos.z = (booz_ins_gps_pos_cm_ned.z * INT32_POS_OF_CM_NUM) / INT32_POS_OF_CM_DEN;
|
||||
booz_ins_ltp_speed.z = (booz_ins_gps_speed_cm_s_ned.z * INT32_SPEED_OF_CM_S_NUM) INT32_SPEED_OF_CM_S_DEN;
|
||||
#endif /* only hf */
|
||||
#else /* hf not used */
|
||||
INT32_VECT2_SCALE_2(booz_ins_ltp_pos, booz_ins_gps_pos_cm_ned, INT32_POS_OF_CM_NUM, INT32_POS_OF_CM_DEN);
|
||||
INT32_VECT2_SCALE_2(booz_ins_ltp_speed, booz_ins_gps_speed_cm_s_ned, INT32_SPEED_OF_CM_S_NUM, INT32_SPEED_OF_CM_S_DEN);
|
||||
#ifndef USE_VFF /* neither hf nor vf used */
|
||||
INT32_VECT3_SCALE_2(booz_ins_ltp_pos, booz_ins_gps_pos_cm_ned, INT32_POS_OF_CM_NUM, INT32_POS_OF_CM_DEN);
|
||||
INT32_VECT3_SCALE_2(booz_ins_ltp_speed, booz_ins_gps_speed_cm_s_ned, INT32_SPEED_OF_CM_S_NUM, INT32_SPEED_OF_CM_S_DEN);
|
||||
#endif /* neither hf nor vf used */
|
||||
#endif /* USE_HFF */
|
||||
|
||||
INT32_VECT3_ENU_OF_NED(booz_ins_enu_pos, booz_ins_ltp_pos);
|
||||
INT32_VECT3_ENU_OF_NED(booz_ins_enu_speed, booz_ins_ltp_speed);
|
||||
INT32_VECT3_ENU_OF_NED(booz_ins_enu_accel, booz_ins_ltp_accel);
|
||||
|
||||
@@ -50,7 +50,6 @@ extern struct EnuCoor_i booz_ins_enu_pos;
|
||||
extern struct EnuCoor_i booz_ins_enu_speed;
|
||||
extern struct EnuCoor_i booz_ins_enu_accel;
|
||||
|
||||
|
||||
extern void booz_ins_init( void );
|
||||
extern void booz_ins_propagate( void );
|
||||
extern void booz_ins_update_baro( void );
|
||||
|
||||
@@ -0,0 +1,363 @@
|
||||
/*
|
||||
* $Id$
|
||||
*
|
||||
* Copyright (C) 2008-2009 Antoine Drouin <poinix@gmail.com>
|
||||
* Copyright (C) 2009 Felix Ruess <felix.ruess@gmail.com>
|
||||
*
|
||||
* This file is part of paparazzi.
|
||||
*
|
||||
* paparazzi 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 2, or (at your option)
|
||||
* any later version.
|
||||
*
|
||||
* paparazzi 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 paparazzi; see the file COPYING. If not, write to
|
||||
* the Free Software Foundation, 59 Temple Place - Suite 330,
|
||||
* Boston, MA 02111-1307, USA.
|
||||
*/
|
||||
|
||||
#include "booz2_hf_float.h"
|
||||
|
||||
/*
|
||||
|
||||
X_x = [ x xdot xbias ]
|
||||
X_y = [ y ydot ybias ]
|
||||
|
||||
|
||||
*/
|
||||
/* initial covariance diagonal */
|
||||
#define INIT_PXX 1.
|
||||
/* process noise (is the same for x and y)*/
|
||||
#define ACCEL_NOISE 0.5
|
||||
#define Q ACCEL_NOISE/512./512./2.
|
||||
#define Qdotdot ACCEL_NOISE/512.
|
||||
#define Qbiasbias 1e-7
|
||||
#define R 1.
|
||||
|
||||
float b2_hff_x;
|
||||
float b2_hff_xbias;
|
||||
float b2_hff_xdot;
|
||||
float b2_hff_xdotdot;
|
||||
|
||||
float b2_hff_y;
|
||||
float b2_hff_ybias;
|
||||
float b2_hff_ydot;
|
||||
float b2_hff_ydotdot;
|
||||
|
||||
float b2_hff_xP[B2_HFF_STATE_SIZE][B2_HFF_STATE_SIZE];
|
||||
float b2_hff_yP[B2_HFF_STATE_SIZE][B2_HFF_STATE_SIZE];
|
||||
|
||||
float b2_hff_x_meas;
|
||||
float b2_hff_y_meas;
|
||||
|
||||
|
||||
static inline void b2_hff_init_x(float init_x, float init_xdot, float init_xbias);
|
||||
static inline void b2_hff_init_y(float init_y, float init_ydot, float init_ybias);
|
||||
|
||||
static inline void b2_hff_propagate_x(float xaccel);
|
||||
static inline void b2_hff_propagate_y(float yaccel);
|
||||
|
||||
static inline void b2_hff_update_x(float x_meas);
|
||||
static inline void b2_hff_update_y(float y_meas);
|
||||
|
||||
static inline void b2_hff_update_xdot(float v);
|
||||
static inline void b2_hff_update_ydot(float v);
|
||||
|
||||
|
||||
|
||||
void b2_hff_init(float init_x, float init_xdot, float init_xbias, float init_y, float init_ydot, float init_ybias) {
|
||||
b2_hff_init_x(init_x, init_xdot, init_xbias);
|
||||
b2_hff_init_y(init_y, init_ydot, init_ybias);
|
||||
}
|
||||
|
||||
static inline void b2_hff_init_x(float init_x, float init_xdot, float init_xbias) {
|
||||
b2_hff_x = init_x;
|
||||
b2_hff_xdot = init_xdot;
|
||||
b2_hff_xbias = init_xbias;
|
||||
int i, j;
|
||||
for (i=0; i<B2_HFF_STATE_SIZE; i++) {
|
||||
for (j=0; j<B2_HFF_STATE_SIZE; j++)
|
||||
b2_hff_xP[i][j] = 0.;
|
||||
b2_hff_xP[i][i] = INIT_PXX;
|
||||
}
|
||||
|
||||
}
|
||||
|
||||
static inline void b2_hff_init_y(float init_y, float init_ydot, float init_ybias) {
|
||||
b2_hff_y = init_y;
|
||||
b2_hff_ydot = init_ydot;
|
||||
b2_hff_ybias = init_ybias;
|
||||
int i, j;
|
||||
for (i=0; i<B2_HFF_STATE_SIZE; i++) {
|
||||
for (j=0; j<B2_HFF_STATE_SIZE; j++)
|
||||
b2_hff_yP[i][j] = 0.;
|
||||
b2_hff_yP[i][i] = INIT_PXX;
|
||||
}
|
||||
|
||||
}
|
||||
|
||||
|
||||
/*
|
||||
|
||||
F = [ 1 dt -dt^2/2
|
||||
0 1 -dt
|
||||
0 0 1 ];
|
||||
|
||||
B = [ dt^2/2 dt 0]';
|
||||
|
||||
Q = [ 0.01 0 0
|
||||
0 0.01 0
|
||||
0 0 0.001 ];
|
||||
|
||||
Xk1 = F * Xk0 + B * accel;
|
||||
|
||||
Pk1 = F * Pk0 * F' + Q;
|
||||
|
||||
*/
|
||||
void b2_hff_propagate(float xaccel, float yaccel) {
|
||||
b2_hff_propagate_x(xaccel);
|
||||
b2_hff_propagate_y(yaccel);
|
||||
}
|
||||
|
||||
static inline void b2_hff_propagate_x(float xaccel) {
|
||||
/* update state */
|
||||
b2_hff_xdotdot = xaccel - b2_hff_xbias;
|
||||
b2_hff_x = b2_hff_x + DT_HFILTER * b2_hff_xdot;
|
||||
b2_hff_xdot = b2_hff_xdot + DT_HFILTER * b2_hff_xdotdot;
|
||||
/* update covariance */
|
||||
const float FPF00 = b2_hff_xP[0][0] + DT_HFILTER * ( b2_hff_xP[1][0] + b2_hff_xP[0][1] + DT_HFILTER * b2_hff_xP[1][1] );
|
||||
const float FPF01 = b2_hff_xP[0][1] + DT_HFILTER * ( b2_hff_xP[1][1] - b2_hff_xP[0][2] - DT_HFILTER * b2_hff_xP[1][2] );
|
||||
const float FPF02 = b2_hff_xP[0][2] + DT_HFILTER * ( b2_hff_xP[1][2] );
|
||||
const float FPF10 = b2_hff_xP[1][0] + DT_HFILTER * (-b2_hff_xP[2][0] + b2_hff_xP[1][1] - DT_HFILTER * b2_hff_xP[2][1] );
|
||||
const float FPF11 = b2_hff_xP[1][1] + DT_HFILTER * (-b2_hff_xP[2][1] - b2_hff_xP[1][2] + DT_HFILTER * b2_hff_xP[2][2] );
|
||||
const float FPF12 = b2_hff_xP[1][2] + DT_HFILTER * (-b2_hff_xP[2][2] );
|
||||
const float FPF20 = b2_hff_xP[2][0] + DT_HFILTER * ( b2_hff_xP[2][1] );
|
||||
const float FPF21 = b2_hff_xP[2][1] + DT_HFILTER * (-b2_hff_xP[2][2] );
|
||||
const float FPF22 = b2_hff_xP[2][2];
|
||||
|
||||
b2_hff_xP[0][0] = FPF00 + Q;
|
||||
b2_hff_xP[0][1] = FPF01;
|
||||
b2_hff_xP[0][2] = FPF02;
|
||||
b2_hff_xP[1][0] = FPF10;
|
||||
b2_hff_xP[1][1] = FPF11 + Qdotdot;
|
||||
b2_hff_xP[1][2] = FPF12;
|
||||
b2_hff_xP[2][0] = FPF20;
|
||||
b2_hff_xP[2][1] = FPF21;
|
||||
b2_hff_xP[2][2] = FPF22 + Qbiasbias;
|
||||
|
||||
}
|
||||
|
||||
static inline void b2_hff_propagate_y(float yaccel) {
|
||||
/* update state */
|
||||
b2_hff_ydotdot = yaccel - b2_hff_ybias;
|
||||
b2_hff_y = b2_hff_y + DT_HFILTER * b2_hff_ydot;
|
||||
b2_hff_ydot = b2_hff_ydot + DT_HFILTER * b2_hff_ydotdot;
|
||||
/* update covariance */
|
||||
const float FPF00 = b2_hff_yP[0][0] + DT_HFILTER * ( b2_hff_yP[1][0] + b2_hff_yP[0][1] + DT_HFILTER * b2_hff_yP[1][1] );
|
||||
const float FPF01 = b2_hff_yP[0][1] + DT_HFILTER * ( b2_hff_yP[1][1] - b2_hff_yP[0][2] - DT_HFILTER * b2_hff_yP[1][2] );
|
||||
const float FPF02 = b2_hff_yP[0][2] + DT_HFILTER * ( b2_hff_yP[1][2] );
|
||||
const float FPF10 = b2_hff_yP[1][0] + DT_HFILTER * (-b2_hff_yP[2][0] + b2_hff_yP[1][1] - DT_HFILTER * b2_hff_yP[2][1] );
|
||||
const float FPF11 = b2_hff_yP[1][1] + DT_HFILTER * (-b2_hff_yP[2][1] - b2_hff_yP[1][2] + DT_HFILTER * b2_hff_yP[2][2] );
|
||||
const float FPF12 = b2_hff_yP[1][2] + DT_HFILTER * (-b2_hff_yP[2][2] );
|
||||
const float FPF20 = b2_hff_yP[2][0] + DT_HFILTER * ( b2_hff_yP[2][1] );
|
||||
const float FPF21 = b2_hff_yP[2][1] + DT_HFILTER * (-b2_hff_yP[2][2] );
|
||||
const float FPF22 = b2_hff_yP[2][2];
|
||||
|
||||
b2_hff_yP[0][0] = FPF00 + Q;
|
||||
b2_hff_yP[0][1] = FPF01;
|
||||
b2_hff_yP[0][2] = FPF02;
|
||||
b2_hff_yP[1][0] = FPF10;
|
||||
b2_hff_yP[1][1] = FPF11 + Qdotdot;
|
||||
b2_hff_yP[1][2] = FPF12;
|
||||
b2_hff_yP[2][0] = FPF20;
|
||||
b2_hff_yP[2][1] = FPF21;
|
||||
b2_hff_yP[2][2] = FPF22 + Qbiasbias;
|
||||
|
||||
}
|
||||
|
||||
|
||||
|
||||
/*
|
||||
H = [1 0 0];
|
||||
R = 0.1;
|
||||
// state residual
|
||||
y = pos_measurement - H * Xm;
|
||||
// covariance residual
|
||||
S = H*Pm*H' + R;
|
||||
// kalman gain
|
||||
K = Pm*H'*inv(S);
|
||||
// update state
|
||||
Xp = Xm + K*y;
|
||||
// update covariance
|
||||
Pp = Pm - K*H*Pm;
|
||||
*/
|
||||
void b2_hff_update_pos (float posx, float posy) {
|
||||
b2_hff_update_x(posx);
|
||||
b2_hff_update_y(posy);
|
||||
}
|
||||
|
||||
static inline void b2_hff_update_x(float x_meas) {
|
||||
b2_hff_x_meas = x_meas;
|
||||
|
||||
const float y = x_meas - b2_hff_x;
|
||||
const float S = b2_hff_xP[0][0] + R;
|
||||
const float K1 = b2_hff_xP[0][0] * 1/S;
|
||||
const float K2 = b2_hff_xP[1][0] * 1/S;
|
||||
const float K3 = b2_hff_xP[2][0] * 1/S;
|
||||
|
||||
b2_hff_x = b2_hff_x + K1 * y;
|
||||
b2_hff_xdot = b2_hff_xdot + K2 * y;
|
||||
b2_hff_xbias = b2_hff_xbias + K3 * y;
|
||||
|
||||
const float P11 = (1. - K1) * b2_hff_xP[0][0];
|
||||
const float P12 = (1. - K1) * b2_hff_xP[0][1];
|
||||
const float P13 = (1. - K1) * b2_hff_xP[0][2];
|
||||
const float P21 = -K2 * b2_hff_xP[0][0] + b2_hff_xP[1][0];
|
||||
const float P22 = -K2 * b2_hff_xP[0][1] + b2_hff_xP[1][1];
|
||||
const float P23 = -K2 * b2_hff_xP[0][2] + b2_hff_xP[1][2];
|
||||
const float P31 = -K3 * b2_hff_xP[0][0] + b2_hff_xP[2][0];
|
||||
const float P32 = -K3 * b2_hff_xP[0][1] + b2_hff_xP[2][1];
|
||||
const float P33 = -K3 * b2_hff_xP[0][2] + b2_hff_xP[2][2];
|
||||
|
||||
b2_hff_xP[0][0] = P11;
|
||||
b2_hff_xP[0][1] = P12;
|
||||
b2_hff_xP[0][2] = P13;
|
||||
b2_hff_xP[1][0] = P21;
|
||||
b2_hff_xP[1][1] = P22;
|
||||
b2_hff_xP[1][2] = P23;
|
||||
b2_hff_xP[2][0] = P31;
|
||||
b2_hff_xP[2][1] = P32;
|
||||
b2_hff_xP[2][2] = P33;
|
||||
|
||||
}
|
||||
|
||||
static inline void b2_hff_update_y(float y_meas) {
|
||||
b2_hff_y_meas = y_meas;
|
||||
|
||||
const float y = y_meas - b2_hff_y;
|
||||
const float S = b2_hff_yP[0][0] + R;
|
||||
const float K1 = b2_hff_yP[0][0] * 1/S;
|
||||
const float K2 = b2_hff_yP[1][0] * 1/S;
|
||||
const float K3 = b2_hff_yP[2][0] * 1/S;
|
||||
|
||||
b2_hff_y = b2_hff_y + K1 * y;
|
||||
b2_hff_ydot = b2_hff_ydot + K2 * y;
|
||||
b2_hff_ybias = b2_hff_ybias + K3 * y;
|
||||
|
||||
const float P11 = (1. - K1) * b2_hff_yP[0][0];
|
||||
const float P12 = (1. - K1) * b2_hff_yP[0][1];
|
||||
const float P13 = (1. - K1) * b2_hff_yP[0][2];
|
||||
const float P21 = -K2 * b2_hff_yP[0][0] + b2_hff_yP[1][0];
|
||||
const float P22 = -K2 * b2_hff_yP[0][1] + b2_hff_yP[1][1];
|
||||
const float P23 = -K2 * b2_hff_yP[0][2] + b2_hff_yP[1][2];
|
||||
const float P31 = -K3 * b2_hff_yP[0][0] + b2_hff_yP[2][0];
|
||||
const float P32 = -K3 * b2_hff_yP[0][1] + b2_hff_yP[2][1];
|
||||
const float P33 = -K3 * b2_hff_yP[0][2] + b2_hff_yP[2][2];
|
||||
|
||||
b2_hff_yP[0][0] = P11;
|
||||
b2_hff_yP[0][1] = P12;
|
||||
b2_hff_yP[0][2] = P13;
|
||||
b2_hff_yP[1][0] = P21;
|
||||
b2_hff_yP[1][1] = P22;
|
||||
b2_hff_yP[1][2] = P23;
|
||||
b2_hff_yP[2][0] = P31;
|
||||
b2_hff_yP[2][1] = P32;
|
||||
b2_hff_yP[2][2] = P33;
|
||||
|
||||
}
|
||||
|
||||
|
||||
|
||||
|
||||
/*
|
||||
H = [0 1 0];
|
||||
R = 0.1;
|
||||
// state residual
|
||||
yd = vx - H * Xm;
|
||||
// covariance residual
|
||||
S = H*Pm*H' + R;
|
||||
// kalman gain
|
||||
K = Pm*H'*inv(S);
|
||||
// update state
|
||||
Xp = Xm + K*yd;
|
||||
// update covariance
|
||||
Pp = Pm - K*H*Pm;
|
||||
*/
|
||||
void b2_hff_update_v(float xspeed, float yspeed) {
|
||||
b2_hff_update_xdot(xspeed);
|
||||
b2_hff_update_ydot(yspeed);
|
||||
}
|
||||
|
||||
static inline void b2_hff_update_xdot(float v) {
|
||||
const float yd = v - b2_hff_xdot;
|
||||
const float S = b2_hff_xP[1][1] + R;
|
||||
const float K1 = b2_hff_xP[0][1] * 1/S;
|
||||
const float K2 = b2_hff_xP[1][1] * 1/S;
|
||||
const float K3 = b2_hff_xP[2][1] * 1/S;
|
||||
|
||||
b2_hff_x = b2_hff_x + K1 * yd;
|
||||
b2_hff_xdot = b2_hff_xdot + K2 * yd;
|
||||
b2_hff_xbias = b2_hff_xbias + K3 * yd;
|
||||
|
||||
const float P11 = -K1 * b2_hff_xP[1][0] + b2_hff_xP[0][0];
|
||||
const float P12 = -K1 * b2_hff_xP[1][1] + b2_hff_xP[0][1];
|
||||
const float P13 = -K1 * b2_hff_xP[1][2] + b2_hff_xP[0][2];
|
||||
const float P21 = (1. - K2) * b2_hff_xP[1][0];
|
||||
const float P22 = (1. - K2) * b2_hff_xP[1][1];
|
||||
const float P23 = (1. - K2) * b2_hff_xP[1][2];
|
||||
const float P31 = -K3 * b2_hff_xP[1][0] + b2_hff_xP[2][0];
|
||||
const float P32 = -K3 * b2_hff_xP[1][1] + b2_hff_xP[2][1];
|
||||
const float P33 = -K3 * b2_hff_xP[1][2] + b2_hff_xP[2][2];
|
||||
|
||||
b2_hff_xP[0][0] = P11;
|
||||
b2_hff_xP[0][1] = P12;
|
||||
b2_hff_xP[0][2] = P13;
|
||||
b2_hff_xP[1][0] = P21;
|
||||
b2_hff_xP[1][1] = P22;
|
||||
b2_hff_xP[1][2] = P23;
|
||||
b2_hff_xP[2][0] = P31;
|
||||
b2_hff_xP[2][1] = P32;
|
||||
b2_hff_xP[2][2] = P33;
|
||||
|
||||
}
|
||||
|
||||
static inline void b2_hff_update_ydot(float v) {
|
||||
const float yd = v - b2_hff_ydot;
|
||||
const float S = b2_hff_yP[1][1] + R;
|
||||
const float K1 = b2_hff_yP[0][1] * 1/S;
|
||||
const float K2 = b2_hff_yP[1][1] * 1/S;
|
||||
const float K3 = b2_hff_yP[2][1] * 1/S;
|
||||
|
||||
b2_hff_y = b2_hff_y + K1 * yd;
|
||||
b2_hff_ydot = b2_hff_ydot + K2 * yd;
|
||||
b2_hff_ybias = b2_hff_ybias + K3 * yd;
|
||||
|
||||
const float P11 = -K1 * b2_hff_yP[1][0] + b2_hff_yP[0][0];
|
||||
const float P12 = -K1 * b2_hff_yP[1][1] + b2_hff_yP[0][1];
|
||||
const float P13 = -K1 * b2_hff_yP[1][2] + b2_hff_yP[0][2];
|
||||
const float P21 = (1. - K2) * b2_hff_yP[1][0];
|
||||
const float P22 = (1. - K2) * b2_hff_yP[1][1];
|
||||
const float P23 = (1. - K2) * b2_hff_yP[1][2];
|
||||
const float P31 = -K3 * b2_hff_yP[1][0] + b2_hff_yP[2][0];
|
||||
const float P32 = -K3 * b2_hff_yP[1][1] + b2_hff_yP[2][1];
|
||||
const float P33 = -K3 * b2_hff_yP[1][2] + b2_hff_yP[2][2];
|
||||
|
||||
b2_hff_yP[0][0] = P11;
|
||||
b2_hff_yP[0][1] = P12;
|
||||
b2_hff_yP[0][2] = P13;
|
||||
b2_hff_yP[1][0] = P21;
|
||||
b2_hff_yP[1][1] = P22;
|
||||
b2_hff_yP[1][2] = P23;
|
||||
b2_hff_yP[2][0] = P31;
|
||||
b2_hff_yP[2][1] = P32;
|
||||
b2_hff_yP[2][2] = P33;
|
||||
|
||||
}
|
||||
|
||||
|
||||
@@ -0,0 +1,54 @@
|
||||
/*
|
||||
* $Id$
|
||||
*
|
||||
* Copyright (C) 2008-2009 Antoine Drouin <poinix@gmail.com>
|
||||
*
|
||||
* This file is part of paparazzi.
|
||||
*
|
||||
* paparazzi 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 2, or (at your option)
|
||||
* any later version.
|
||||
*
|
||||
* paparazzi 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 paparazzi; see the file COPYING. If not, write to
|
||||
* the Free Software Foundation, 59 Temple Place - Suite 330,
|
||||
* Boston, MA 02111-1307, USA.
|
||||
*/
|
||||
|
||||
#ifndef BOOZ2_HF_FLOAT_H
|
||||
#define BOOZ2_HF_FLOAT_H
|
||||
|
||||
#define B2_HFF_STATE_SIZE 3
|
||||
|
||||
#define B2_HFF_UPDATE_SPEED
|
||||
#define B2_HFF_UPDATE_POS
|
||||
|
||||
extern float b2_hff_x;
|
||||
extern float b2_hff_xbias;
|
||||
extern float b2_hff_xdot;
|
||||
extern float b2_hff_xdotdot;
|
||||
|
||||
extern float b2_hff_y;
|
||||
extern float b2_hff_ybias;
|
||||
extern float b2_hff_ydot;
|
||||
extern float b2_hff_ydotdot;
|
||||
|
||||
extern float b2_hff_xP[B2_HFF_STATE_SIZE][B2_HFF_STATE_SIZE];
|
||||
extern float b2_hff_yP[B2_HFF_STATE_SIZE][B2_HFF_STATE_SIZE];
|
||||
|
||||
extern float b2_hff_x_meas;
|
||||
extern float b2_hff_y_meas;
|
||||
|
||||
extern void b2_hff_init(float init_x, float init_xdot, float init_xbias, float init_y, float init_ydot, float init_ybias);
|
||||
extern void b2_hff_propagate(float xaccel, float yaccel);
|
||||
extern void b2_hff_update_pos(float xpos, float ypos);
|
||||
extern void b2_hff_update_v(float xspeed, float yspeed);
|
||||
|
||||
#endif /* BOOZ2_HF_FLOAT_H */
|
||||
|
||||
@@ -73,6 +73,9 @@ struct FloatRates {
|
||||
}
|
||||
|
||||
|
||||
/*
|
||||
* Dimension 3 Vectors
|
||||
*/
|
||||
|
||||
#define FLOAT_VECT3_ZERO(_v) VECT3_ASSIGN(_v, 0., 0., 0.) \
|
||||
|
||||
|
||||
@@ -177,6 +177,11 @@ struct Int64Vect3 {
|
||||
(_o).y = ((_i).y << (_l)); \
|
||||
}
|
||||
|
||||
#define INT32_VECT2_SCALE_2(_a, _b, _num, _den) { \
|
||||
(_a).x = ((_b).x * (_num)) / (_den); \
|
||||
(_a).y = ((_b).y * (_num)) / (_den); \
|
||||
}
|
||||
|
||||
/*
|
||||
* Dimension 3 Vectors
|
||||
*/
|
||||
@@ -288,8 +293,8 @@ struct Int64Vect3 {
|
||||
}
|
||||
|
||||
|
||||
/*
|
||||
* http://www.mathworks.com/access/helpdesk_r13/help/toolbox/aeroblks/quaternionstodirectioncosinematrix.html
|
||||
/*
|
||||
* http://www.mathworks.com/access/helpdesk_r13/help/toolbox/aeroblks/quaternionstodirectioncosinematrix.html
|
||||
*/
|
||||
#define INT32_RMAT_OF_QUAT(_rm, _q) { \
|
||||
const int32_t qx2 = INT_MULT_RSHIFT((_q).qx,(_q).qx, INT32_QUAT_FRAC); \
|
||||
|
||||
Reference in New Issue
Block a user