Mirrors_Framework/paparazzi
1
0
Fork 0
mirror of https://github.com/paparazzi/paparazzi.git synced 2026-06-05 23:49:00 +08:00
Code Issues Actions 28 Packages Projects Releases Wiki Activity

rename b2_vff/B2_VFF and booz2_vff/BOOZ2_VFF to vff/VFF

Browse Source
This commit is contained in:
Felix Ruess
2010-09-28 14:05:27 +00:00
parent 4c67137799
commit 21dce537c6
9 changed files with 221 additions and 221 deletions
Download Patch File Download Diff File Expand all files Collapse all files
conf/messages.xml
+1 -1
View File
@@ -1158,7 +1158,7 @@
<field name="value_filtered" type="uint16"/>
</message>
<message name="BOOZ2_VFF" id="162">
<message name="VFF" id="162">
<field name="measure" type="float"/>
<field name="z" type="float"/>
<field name="zd" type="float"/>
conf/telemetry/telemetry_booz2.xml
+1 -1
View File
@@ -75,7 +75,7 @@
<message name="BOOZ_STATUS" period="1.2"/>
<message name="DL_VALUE" period="0.5"/>
<message name="ALIVE" period="0.9"/>
<message name="BOOZ2_VFF" period=".05"/>
<message name="VFF" period=".05"/>
<message name="BOOZ2_VERT_LOOP" period=".05"/>
<!-- <message name="BOOZ2_CMD" period=".05"/> -->
<message name="INS" period=".05"/>
conf/telemetry/telemetry_booz2_flixr.xml
+1 -1
View File
@@ -74,7 +74,7 @@
<message name="BOOZ_STATUS" period="1.2"/>
<message name="DL_VALUE" period="0.5"/>
<message name="ALIVE" period="0.9"/>
<message name="BOOZ2_VFF" period=".1"/>
<message name="VFF" period=".1"/>
<message name="BOOZ2_VERT_LOOP" period=".1"/>
<!-- <message name="BOOZ2_CMD" period=".05"/> -->
<message name="INS" period=".2"/>
sw/airborne/booz/booz2_telemetry.h
+10 -10
View File
@@ -473,18 +473,18 @@ extern uint8_t telemetry_mode_Main_DefaultChannel;
#ifdef USE_VFF
#include "ins/vf_float.h"
#define PERIODIC_SEND_BOOZ2_VFF(_chan) { \
DOWNLINK_SEND_BOOZ2_VFF(_chan, \
&b2_vff_z_meas, \
&b2_vff_z, \
&b2_vff_zdot, \
&b2_vff_bias, \
& b2_vff_P[0][0], \
& b2_vff_P[1][1], \
& b2_vff_P[2][2]); \
#define PERIODIC_SEND_VFF(_chan) { \
DOWNLINK_SEND_VFF(_chan, \
&vff_z_meas, \
&vff_z, \
&vff_zdot, \
&vff_bias, \
& vff_P[0][0], \
& vff_P[1][1], \
& vff_P[2][2]); \
}
#else
#define PERIODIC_SEND_BOOZ2_VFF(_chan) {}
#define PERIODIC_SEND_VFF(_chan) {}
#endif
#ifdef USE_HFF
sw/airborne/firmwares/rotorcraft/ins.c
+11 -11
View File
@@ -113,7 +113,7 @@ void ins_init() {
#ifdef BOOZ2_SONAR
ins_update_on_agl = FALSE;
#endif
b2_vff_init(0., 0., 0.);
vff_init(0., 0., 0.);
#endif
ins_vf_realign = FALSE;
ins_hf_realign = FALSE;
@@ -142,7 +142,7 @@ void ins_realign_h(struct FloatVect2 pos __attribute__ ((unused)), struct FloatV
void ins_realign_v(float z) {
#ifdef USE_VFF
b2_vff_realign(z);
vff_realign(z);
#endif
}
@@ -156,10 +156,10 @@ void ins_propagate() {
#ifdef USE_VFF
if (baro.status == BS_RUNNING && ins_baro_initialised) {
b2_vff_propagate(z_accel_float);
ins_ltp_accel.z = ACCEL_BFP_OF_REAL(b2_vff_zdotdot);
ins_ltp_speed.z = SPEED_BFP_OF_REAL(b2_vff_zdot);
ins_ltp_pos.z = POS_BFP_OF_REAL(b2_vff_z);
vff_propagate(z_accel_float);
ins_ltp_accel.z = ACCEL_BFP_OF_REAL(vff_zdotdot);
ins_ltp_speed.z = SPEED_BFP_OF_REAL(vff_zdot);
ins_ltp_pos.z = POS_BFP_OF_REAL(vff_z);
}
else { // feed accel from the sensors
ins_ltp_accel.z = ACCEL_BFP_OF_REAL(z_accel_float);
@@ -196,15 +196,15 @@ void ins_update_baro() {
#ifdef BOOZ2_SONAR
ins_sonar_offset = sonar_meas;
#endif
b2_vff_realign(0.);
ins_ltp_accel.z = ACCEL_BFP_OF_REAL(b2_vff_zdotdot);
ins_ltp_speed.z = SPEED_BFP_OF_REAL(b2_vff_zdot);
ins_ltp_pos.z = POS_BFP_OF_REAL(b2_vff_z);
vff_realign(0.);
ins_ltp_accel.z = ACCEL_BFP_OF_REAL(vff_zdotdot);
ins_ltp_speed.z = SPEED_BFP_OF_REAL(vff_zdot);
ins_ltp_pos.z = POS_BFP_OF_REAL(vff_z);
ins_enu_pos.z = -ins_ltp_pos.z;
ins_enu_speed.z = -ins_ltp_speed.z;
ins_enu_accel.z = -ins_ltp_accel.z;
}
b2_vff_update(alt_float);
vff_update(alt_float);
}
#endif
}
sw/airborne/firmwares/rotorcraft/ins/vf_float.c
+95 -95
View File
@@ -41,24 +41,24 @@ temps :
#define Qbiasbias 1e-7
#define R 1.
float b2_vff_z;
float b2_vff_bias;
float b2_vff_zdot;
float b2_vff_zdotdot;
float vff_z;
float vff_bias;
float vff_zdot;
float vff_zdotdot;
float b2_vff_P[B2_VFF_STATE_SIZE][B2_VFF_STATE_SIZE];
float vff_P[VFF_STATE_SIZE][VFF_STATE_SIZE];
float b2_vff_z_meas;
float vff_z_meas;
void b2_vff_init(float init_z, float init_zdot, float init_bias) {
b2_vff_z = init_z;
b2_vff_zdot = init_zdot;
b2_vff_bias = init_bias;
void vff_init(float init_z, float init_zdot, float init_bias) {
vff_z = init_z;
vff_zdot = init_zdot;
vff_bias = init_bias;
int i, j;
for (i=0; i<B2_VFF_STATE_SIZE; i++) {
for (j=0; j<B2_VFF_STATE_SIZE; j++)
b2_vff_P[i][j] = 0.;
b2_vff_P[i][i] = INIT_PXX;
for (i=0; i<VFF_STATE_SIZE; i++) {
for (j=0; j<VFF_STATE_SIZE; j++)
vff_P[i][j] = 0.;
vff_P[i][i] = INIT_PXX;
}
}
@@ -81,31 +81,31 @@ void b2_vff_init(float init_z, float init_zdot, float init_bias) {
Pk1 = F * Pk0 * F' + Q;
*/
void b2_vff_propagate(float accel) {
void vff_propagate(float accel) {
/* update state */
b2_vff_zdotdot = accel + 9.81 - b2_vff_bias;
b2_vff_z = b2_vff_z + DT_VFILTER * b2_vff_zdot;
b2_vff_zdot = b2_vff_zdot + DT_VFILTER * b2_vff_zdotdot;
vff_zdotdot = accel + 9.81 - vff_bias;
vff_z = vff_z + DT_VFILTER * vff_zdot;
vff_zdot = vff_zdot + DT_VFILTER * vff_zdotdot;
/* update covariance */
const float FPF00 = b2_vff_P[0][0] + DT_VFILTER * ( b2_vff_P[1][0] + b2_vff_P[0][1] + DT_VFILTER * b2_vff_P[1][1] );
const float FPF01 = b2_vff_P[0][1] + DT_VFILTER * ( b2_vff_P[1][1] - b2_vff_P[0][2] - DT_VFILTER * b2_vff_P[1][2] );
const float FPF02 = b2_vff_P[0][2] + DT_VFILTER * ( b2_vff_P[1][2] );
const float FPF10 = b2_vff_P[1][0] + DT_VFILTER * (-b2_vff_P[2][0] + b2_vff_P[1][1] - DT_VFILTER * b2_vff_P[2][1] );
const float FPF11 = b2_vff_P[1][1] + DT_VFILTER * (-b2_vff_P[2][1] - b2_vff_P[1][2] + DT_VFILTER * b2_vff_P[2][2] );
const float FPF12 = b2_vff_P[1][2] + DT_VFILTER * (-b2_vff_P[2][2] );
const float FPF20 = b2_vff_P[2][0] + DT_VFILTER * ( b2_vff_P[2][1] );
const float FPF21 = b2_vff_P[2][1] + DT_VFILTER * (-b2_vff_P[2][2] );
const float FPF22 = b2_vff_P[2][2];
const float FPF00 = vff_P[0][0] + DT_VFILTER * ( vff_P[1][0] + vff_P[0][1] + DT_VFILTER * vff_P[1][1] );
const float FPF01 = vff_P[0][1] + DT_VFILTER * ( vff_P[1][1] - vff_P[0][2] - DT_VFILTER * vff_P[1][2] );
const float FPF02 = vff_P[0][2] + DT_VFILTER * ( vff_P[1][2] );
const float FPF10 = vff_P[1][0] + DT_VFILTER * (-vff_P[2][0] + vff_P[1][1] - DT_VFILTER * vff_P[2][1] );
const float FPF11 = vff_P[1][1] + DT_VFILTER * (-vff_P[2][1] - vff_P[1][2] + DT_VFILTER * vff_P[2][2] );
const float FPF12 = vff_P[1][2] + DT_VFILTER * (-vff_P[2][2] );
const float FPF20 = vff_P[2][0] + DT_VFILTER * ( vff_P[2][1] );
const float FPF21 = vff_P[2][1] + DT_VFILTER * (-vff_P[2][2] );
const float FPF22 = vff_P[2][2];
b2_vff_P[0][0] = FPF00 + Qzz;
b2_vff_P[0][1] = FPF01;
b2_vff_P[0][2] = FPF02;
b2_vff_P[1][0] = FPF10;
b2_vff_P[1][1] = FPF11 + Qzdotzdot;
b2_vff_P[1][2] = FPF12;
b2_vff_P[2][0] = FPF20;
b2_vff_P[2][1] = FPF21;
b2_vff_P[2][2] = FPF22 + Qbiasbias;
vff_P[0][0] = FPF00 + Qzz;
vff_P[0][1] = FPF01;
vff_P[0][2] = FPF02;
vff_P[1][0] = FPF10;
vff_P[1][1] = FPF11 + Qzdotzdot;
vff_P[1][2] = FPF12;
vff_P[2][0] = FPF20;
vff_P[2][1] = FPF21;
vff_P[2][2] = FPF22 + Qbiasbias;
}
/*
@@ -123,45 +123,45 @@ void b2_vff_propagate(float accel) {
Pp = Pm - K*H*Pm;
*/
static inline void update_z_conf(float z_meas, float conf) {
b2_vff_z_meas = z_meas;
vff_z_meas = z_meas;
const float y = z_meas - b2_vff_z;
const float S = b2_vff_P[0][0] + conf;
const float K1 = b2_vff_P[0][0] * 1/S;
const float K2 = b2_vff_P[1][0] * 1/S;
const float K3 = b2_vff_P[2][0] * 1/S;
const float y = z_meas - vff_z;
const float S = vff_P[0][0] + conf;
const float K1 = vff_P[0][0] * 1/S;
const float K2 = vff_P[1][0] * 1/S;
const float K3 = vff_P[2][0] * 1/S;
b2_vff_z = b2_vff_z + K1 * y;
b2_vff_zdot = b2_vff_zdot + K2 * y;
b2_vff_bias = b2_vff_bias + K3 * y;
vff_z = vff_z + K1 * y;
vff_zdot = vff_zdot + K2 * y;
vff_bias = vff_bias + K3 * y;
const float P11 = (1. - K1) * b2_vff_P[0][0];
const float P12 = (1. - K1) * b2_vff_P[0][1];
const float P13 = (1. - K1) * b2_vff_P[0][2];
const float P21 = -K2 * b2_vff_P[0][0] + b2_vff_P[1][0];
const float P22 = -K2 * b2_vff_P[0][1] + b2_vff_P[1][1];
const float P23 = -K2 * b2_vff_P[0][2] + b2_vff_P[1][2];
const float P31 = -K3 * b2_vff_P[0][0] + b2_vff_P[2][0];
const float P32 = -K3 * b2_vff_P[0][1] + b2_vff_P[2][1];
const float P33 = -K3 * b2_vff_P[0][2] + b2_vff_P[2][2];
const float P11 = (1. - K1) * vff_P[0][0];
const float P12 = (1. - K1) * vff_P[0][1];
const float P13 = (1. - K1) * vff_P[0][2];
const float P21 = -K2 * vff_P[0][0] + vff_P[1][0];
const float P22 = -K2 * vff_P[0][1] + vff_P[1][1];
const float P23 = -K2 * vff_P[0][2] + vff_P[1][2];
const float P31 = -K3 * vff_P[0][0] + vff_P[2][0];
const float P32 = -K3 * vff_P[0][1] + vff_P[2][1];
const float P33 = -K3 * vff_P[0][2] + vff_P[2][2];
b2_vff_P[0][0] = P11;
b2_vff_P[0][1] = P12;
b2_vff_P[0][2] = P13;
b2_vff_P[1][0] = P21;
b2_vff_P[1][1] = P22;
b2_vff_P[1][2] = P23;
b2_vff_P[2][0] = P31;
b2_vff_P[2][1] = P32;
b2_vff_P[2][2] = P33;
vff_P[0][0] = P11;
vff_P[0][1] = P12;
vff_P[0][2] = P13;
vff_P[1][0] = P21;
vff_P[1][1] = P22;
vff_P[1][2] = P23;
vff_P[2][0] = P31;
vff_P[2][1] = P32;
vff_P[2][2] = P33;
}
void b2_vff_update(float z_meas) {
void vff_update(float z_meas) {
update_z_conf(z_meas, R);
}
void b2_vff_update_z_conf(float z_meas, float conf) {
void vff_update_z_conf(float z_meas, float conf) {
update_z_conf(z_meas, conf);
}
@@ -180,43 +180,43 @@ void b2_vff_update_z_conf(float z_meas, float conf) {
Pp = Pm - K*H*Pm;
*/
static inline void update_vz_conf(float vz, float conf) {
const float yd = vz - b2_vff_zdot;
const float S = b2_vff_P[1][1] + conf;
const float K1 = b2_vff_P[0][1] * 1/S;
const float K2 = b2_vff_P[1][1] * 1/S;
const float K3 = b2_vff_P[2][1] * 1/S;
const float yd = vz - vff_zdot;
const float S = vff_P[1][1] + conf;
const float K1 = vff_P[0][1] * 1/S;
const float K2 = vff_P[1][1] * 1/S;
const float K3 = vff_P[2][1] * 1/S;
b2_vff_z = b2_vff_z + K1 * yd;
b2_vff_zdot = b2_vff_zdot + K2 * yd;
b2_vff_bias = b2_vff_bias + K3 * yd;
vff_z = vff_z + K1 * yd;
vff_zdot = vff_zdot + K2 * yd;
vff_bias = vff_bias + K3 * yd;
const float P11 = -K1 * b2_vff_P[1][0] + b2_vff_P[0][0];
const float P12 = -K1 * b2_vff_P[1][1] + b2_vff_P[0][1];
const float P13 = -K1 * b2_vff_P[1][2] + b2_vff_P[0][2];
const float P21 = (1. - K2) * b2_vff_P[1][0];
const float P22 = (1. - K2) * b2_vff_P[1][1];
const float P23 = (1. - K2) * b2_vff_P[1][2];
const float P31 = -K3 * b2_vff_P[1][0] + b2_vff_P[2][0];
const float P32 = -K3 * b2_vff_P[1][1] + b2_vff_P[2][1];
const float P33 = -K3 * b2_vff_P[1][2] + b2_vff_P[2][2];
const float P11 = -K1 * vff_P[1][0] + vff_P[0][0];
const float P12 = -K1 * vff_P[1][1] + vff_P[0][1];
const float P13 = -K1 * vff_P[1][2] + vff_P[0][2];
const float P21 = (1. - K2) * vff_P[1][0];
const float P22 = (1. - K2) * vff_P[1][1];
const float P23 = (1. - K2) * vff_P[1][2];
const float P31 = -K3 * vff_P[1][0] + vff_P[2][0];
const float P32 = -K3 * vff_P[1][1] + vff_P[2][1];
const float P33 = -K3 * vff_P[1][2] + vff_P[2][2];
b2_vff_P[0][0] = P11;
b2_vff_P[0][1] = P12;
b2_vff_P[0][2] = P13;
b2_vff_P[1][0] = P21;
b2_vff_P[1][1] = P22;
b2_vff_P[1][2] = P23;
b2_vff_P[2][0] = P31;
b2_vff_P[2][1] = P32;
b2_vff_P[2][2] = P33;
vff_P[0][0] = P11;
vff_P[0][1] = P12;
vff_P[0][2] = P13;
vff_P[1][0] = P21;
vff_P[1][1] = P22;
vff_P[1][2] = P23;
vff_P[2][0] = P31;
vff_P[2][1] = P32;
vff_P[2][2] = P33;
}
void b2_vff_update_vz_conf(float vz_meas, float conf) {
void vff_update_vz_conf(float vz_meas, float conf) {
update_vz_conf(vz_meas, conf);
}
void b2_vff_realign(float z_meas) {
b2_vff_z = z_meas;
b2_vff_zdot = 0;
void vff_realign(float z_meas) {
vff_z = z_meas;
vff_zdot = 0;
}
sw/airborne/firmwares/rotorcraft/ins/vf_float.h
+16 -16
View File
@@ -21,24 +21,24 @@
* Boston, MA 02111-1307, USA.
*/
#ifndef BOOZ2_VF_FLOAT_H
#define BOOZ2_VF_FLOAT_H
#ifndef VF_FLOAT_H
#define VF_FLOAT_H
#define B2_VFF_STATE_SIZE 3
#define VFF_STATE_SIZE 3
extern float b2_vff_z;
extern float b2_vff_zdot;
extern float b2_vff_bias;
extern float b2_vff_P[B2_VFF_STATE_SIZE][B2_VFF_STATE_SIZE];
extern float b2_vff_zdotdot;
extern float vff_z;
extern float vff_zdot;
extern float vff_bias;
extern float vff_P[VFF_STATE_SIZE][VFF_STATE_SIZE];
extern float vff_zdotdot;
extern float b2_vff_z_meas;
extern float vff_z_meas;
extern void b2_vff_init(float z, float zdot, float bias);
extern void b2_vff_propagate(float accel);
extern void b2_vff_update(float z_meas);
extern void b2_vff_update_z_conf(float z_meas, float conf);
extern void b2_vff_update_vz_conf(float vz_meas, float conf);
extern void b2_vff_realign(float z_meas);
extern void vff_init(float z, float zdot, float bias);
extern void vff_propagate(float accel);
extern void vff_update(float z_meas);
extern void vff_update_z_conf(float z_meas, float conf);
extern void vff_update_vz_conf(float vz_meas, float conf);
extern void vff_realign(float z_meas);
#endif /* BOOZ2_VF_FLOAT_H */
#endif /* VF_FLOAT_H */
sw/airborne/firmwares/rotorcraft/ins/vf_int.c
+66 -66
View File
@@ -25,42 +25,42 @@
#include "booz_geometry_mixed.h"
int64_t b2_vfi_z;
int32_t b2_vfi_zd;
int32_t b2_vfi_abias;
int32_t b2_vfi_zdd;
int32_t b2_vfi_P[B2_VFI_S_SIZE][B2_VFI_S_SIZE];
int64_t vfi_z;
int32_t vfi_zd;
int32_t vfi_abias;
int32_t vfi_zdd;
int32_t vfi_P[VFI_S_SIZE][VFI_S_SIZE];
/* initial covariance */
#define VFI_INIT_PZZ BOOZ_INT_OF_FLOAT(1., B2_VFI_P_FRAC)
#define VFI_INIT_PZDZD BOOZ_INT_OF_FLOAT(1., B2_VFI_P_FRAC)
#define VFI_INIT_PABAB BOOZ_INT_OF_FLOAT(1., B2_VFI_P_FRAC)
#define VFI_INIT_PZZ BOOZ_INT_OF_FLOAT(1., VFI_P_FRAC)
#define VFI_INIT_PZDZD BOOZ_INT_OF_FLOAT(1., VFI_P_FRAC)
#define VFI_INIT_PABAB BOOZ_INT_OF_FLOAT(1., VFI_P_FRAC)
/* system and measurement noise */
#define VFI_ACCEL_NOISE 0.1
#define VFI_DT2_2 (1./(512.*512.)/2.)
#define VFI_DT (1./512.)
#define VFI_QZZ BOOZ_INT_OF_FLOAT(VFI_ACCEL_NOISE*VFI_DT2_2, B2_VFI_P_FRAC)
#define VFI_QZDZD BOOZ_INT_OF_FLOAT(VFI_ACCEL_NOISE*VFI_DT, B2_VFI_P_FRAC)
#define VFI_QABAB BOOZ_INT_OF_FLOAT(1e-7, B2_VFI_P_FRAC)
#define VFI_R BOOZ_INT_OF_FLOAT(1., B2_VFI_P_FRAC)
#define VFI_QZZ BOOZ_INT_OF_FLOAT(VFI_ACCEL_NOISE*VFI_DT2_2, VFI_P_FRAC)
#define VFI_QZDZD BOOZ_INT_OF_FLOAT(VFI_ACCEL_NOISE*VFI_DT, VFI_P_FRAC)
#define VFI_QABAB BOOZ_INT_OF_FLOAT(1e-7, VFI_P_FRAC)
#define VFI_R BOOZ_INT_OF_FLOAT(1., VFI_P_FRAC)
void vfi_init(int32_t z0, int32_t zd0, int32_t bias0 ) {
// initialize state vector
b2_vfi_z = z0;
b2_vfi_zd = zd0;
b2_vfi_abias = bias0;
b2_vfi_zdd = 0;
vfi_z = z0;
vfi_zd = zd0;
vfi_abias = bias0;
vfi_zdd = 0;
// initialize covariance
int i, j;
for (i=0; i<B2_VFI_S_SIZE; i++)
for (j=0; j<B2_VFI_S_SIZE; j++)
b2_vfi_P[i][j] = 0;
b2_vfi_P[B2_VFI_S_Z][B2_VFI_S_Z] = VFI_INIT_PZZ;
b2_vfi_P[B2_VFI_S_ZD][B2_VFI_S_ZD] = VFI_INIT_PZDZD;
b2_vfi_P[B2_VFI_S_AB][B2_VFI_S_AB] = VFI_INIT_PABAB;
for (i=0; i<VFI_S_SIZE; i++)
for (j=0; j<VFI_S_SIZE; j++)
vfi_P[i][j] = 0;
vfi_P[VFI_S_Z][VFI_S_Z] = VFI_INIT_PZZ;
vfi_P[VFI_S_ZD][VFI_S_ZD] = VFI_INIT_PZDZD;
vfi_P[VFI_S_AB][VFI_S_AB] = VFI_INIT_PABAB;
}
@@ -85,65 +85,65 @@ void vfi_init(int32_t z0, int32_t zd0, int32_t bias0 ) {
void vfi_propagate( int32_t accel_reading ) {
// compute unbiased vertical acceleration
b2_vfi_zdd = accel_reading + BOOZ_INT_OF_FLOAT(9.81, B2_VFI_ZDD_FRAC) - b2_vfi_abias;
vfi_zdd = accel_reading + BOOZ_INT_OF_FLOAT(9.81, VFI_ZDD_FRAC) - vfi_abias;
// propagate state
const int32_t dz = b2_vfi_zd >> ( B2_VFI_F_UPDATE_FRAC + B2_VFI_ZD_FRAC - B2_VFI_Z_FRAC);
b2_vfi_z += dz;
const int32_t dzd = b2_vfi_zdd >> ( B2_VFI_F_UPDATE_FRAC + B2_VFI_ZDD_FRAC - B2_VFI_ZD_FRAC);
b2_vfi_zd += dzd;
const int32_t dz = vfi_zd >> ( VFI_F_UPDATE_FRAC + VFI_ZD_FRAC - VFI_Z_FRAC);
vfi_z += dz;
const int32_t dzd = vfi_zdd >> ( VFI_F_UPDATE_FRAC + VFI_ZDD_FRAC - VFI_ZD_FRAC);
vfi_zd += dzd;
// propagate covariance
const int32_t tmp1 = b2_vfi_P[1][0] + b2_vfi_P[0][1] + (b2_vfi_P[1][1]>>B2_VFI_F_UPDATE_FRAC);
const int32_t FPF00 = b2_vfi_P[0][0] + (tmp1>>B2_VFI_F_UPDATE_FRAC);
const int32_t tmp2 = b2_vfi_P[1][1] - b2_vfi_P[0][2] - (b2_vfi_P[1][2]>>B2_VFI_F_UPDATE_FRAC);
const int32_t FPF01 = b2_vfi_P[0][1] + (tmp2>>B2_VFI_F_UPDATE_FRAC);
const int32_t FPF02 = b2_vfi_P[0][2] + (b2_vfi_P[1][2] >> B2_VFI_F_UPDATE_FRAC);;
const int32_t tmp3 = -b2_vfi_P[2][0] + b2_vfi_P[1][1] - (b2_vfi_P[2][1]>>B2_VFI_F_UPDATE_FRAC);
const int32_t FPF10 = b2_vfi_P[1][0] + (tmp3>>B2_VFI_F_UPDATE_FRAC);
const int32_t tmp4 = -b2_vfi_P[2][1] - b2_vfi_P[1][2] + (b2_vfi_P[2][2]>>B2_VFI_F_UPDATE_FRAC);
const int32_t FPF11 = b2_vfi_P[1][1] + (tmp4>>B2_VFI_F_UPDATE_FRAC);
const int32_t FPF12 = b2_vfi_P[1][2] - (b2_vfi_P[2][2] >> B2_VFI_F_UPDATE_FRAC);
const int32_t FPF20 = b2_vfi_P[2][0] + (b2_vfi_P[2][1] >> B2_VFI_F_UPDATE_FRAC);
const int32_t FPF21 = b2_vfi_P[2][1] - (b2_vfi_P[2][2] >> B2_VFI_F_UPDATE_FRAC);
const int32_t FPF22 = b2_vfi_P[2][2];
const int32_t tmp1 = vfi_P[1][0] + vfi_P[0][1] + (vfi_P[1][1]>>VFI_F_UPDATE_FRAC);
const int32_t FPF00 = vfi_P[0][0] + (tmp1>>VFI_F_UPDATE_FRAC);
const int32_t tmp2 = vfi_P[1][1] - vfi_P[0][2] - (vfi_P[1][2]>>VFI_F_UPDATE_FRAC);
const int32_t FPF01 = vfi_P[0][1] + (tmp2>>VFI_F_UPDATE_FRAC);
const int32_t FPF02 = vfi_P[0][2] + (vfi_P[1][2] >> VFI_F_UPDATE_FRAC);;
const int32_t tmp3 = -vfi_P[2][0] + vfi_P[1][1] - (vfi_P[2][1]>>VFI_F_UPDATE_FRAC);
const int32_t FPF10 = vfi_P[1][0] + (tmp3>>VFI_F_UPDATE_FRAC);
const int32_t tmp4 = -vfi_P[2][1] - vfi_P[1][2] + (vfi_P[2][2]>>VFI_F_UPDATE_FRAC);
const int32_t FPF11 = vfi_P[1][1] + (tmp4>>VFI_F_UPDATE_FRAC);
const int32_t FPF12 = vfi_P[1][2] - (vfi_P[2][2] >> VFI_F_UPDATE_FRAC);
const int32_t FPF20 = vfi_P[2][0] + (vfi_P[2][1] >> VFI_F_UPDATE_FRAC);
const int32_t FPF21 = vfi_P[2][1] - (vfi_P[2][2] >> VFI_F_UPDATE_FRAC);
const int32_t FPF22 = vfi_P[2][2];
b2_vfi_P[0][0] = FPF00 + VFI_QZZ;
b2_vfi_P[0][1] = FPF01;
b2_vfi_P[0][2] = FPF02;
b2_vfi_P[1][0] = FPF10;
b2_vfi_P[1][1] = FPF11 + VFI_QZDZD;
b2_vfi_P[1][2] = FPF12;
b2_vfi_P[2][0] = FPF20;
b2_vfi_P[2][1] = FPF21;
b2_vfi_P[2][2] = FPF22 + VFI_QABAB;
vfi_P[0][0] = FPF00 + VFI_QZZ;
vfi_P[0][1] = FPF01;
vfi_P[0][2] = FPF02;
vfi_P[1][0] = FPF10;
vfi_P[1][1] = FPF11 + VFI_QZDZD;
vfi_P[1][2] = FPF12;
vfi_P[2][0] = FPF20;
vfi_P[2][1] = FPF21;
vfi_P[2][2] = FPF22 + VFI_QABAB;
}
void vfi_update( int32_t z_meas ) {
const int64_t y = (z_meas<<(B2_VFI_Z_FRAC-B2_VFI_MEAS_Z_FRAC)) - b2_vfi_z;
const int32_t S = b2_vfi_P[0][0] + VFI_R;
const int64_t y = (z_meas<<(VFI_Z_FRAC-VFI_MEAS_Z_FRAC)) - vfi_z;
const int32_t S = vfi_P[0][0] + VFI_R;
const int32_t K1 = b2_vfi_P[0][0] / S;
const int32_t K2 = b2_vfi_P[1][0] / S;
const int32_t K3 = b2_vfi_P[2][0] / S;
const int32_t K1 = vfi_P[0][0] / S;
const int32_t K2 = vfi_P[1][0] / S;
const int32_t K3 = vfi_P[2][0] / S;
b2_vfi_z = b2_vfi_z + ((K1 * y)>>B2_VFI_P_FRAC);
b2_vfi_zd = b2_vfi_zd + ((K2 * y)>>B2_VFI_P_FRAC);
b2_vfi_abias = b2_vfi_abias + ((K3 * y)>>B2_VFI_P_FRAC);
vfi_z = vfi_z + ((K1 * y)>>VFI_P_FRAC);
vfi_zd = vfi_zd + ((K2 * y)>>VFI_P_FRAC);
vfi_abias = vfi_abias + ((K3 * y)>>VFI_P_FRAC);
#if 0
const int32_t P11 = ((BOOZ_INT_OF_FLOAT(1., B2_VFI_P_RES) - K1) * b2_vfi_P[0][0])>>B2_VFI_P_RES;
const int32_t P12 = (BOOZ_INT_OF_FLOAT(1., B2_VFI_P_RES) - K1) * b2_vfi_P[0][1];
const int32_t P13 = (BOOZ_INT_OF_FLOAT(1., B2_VFI_P_RES) - K1) * b2_vfi_P[0][2];
const int32_t P21 = -K2 * b2_vfi_P[0][0] + b2_vfi_P[1][0];
const int32_t P22 = -K2 * b2_vfi_P[0][1] + b2_vfi_P[1][1];
const int32_t P23 = -K2 * b2_vfi_P[0][2] + b2_vfi_P[1][2];
const int32_t P31 = -K3 * b2_vfi_P[0][0] + b2_vfi_P[2][0];
const int32_t P32 = -K3 * b2_vfi_P[0][1] + b2_vfi_P[2][1];
const int32_t P33 = -K3 * b2_vfi_P[0][2] + b2_vfi_P[2][2];
const int32_t P11 = ((BOOZ_INT_OF_FLOAT(1., VFI_P_RES) - K1) * vfi_P[0][0])>>VFI_P_RES;
const int32_t P12 = (BOOZ_INT_OF_FLOAT(1., VFI_P_RES) - K1) * vfi_P[0][1];
const int32_t P13 = (BOOZ_INT_OF_FLOAT(1., VFI_P_RES) - K1) * vfi_P[0][2];
const int32_t P21 = -K2 * vfi_P[0][0] + vfi_P[1][0];
const int32_t P22 = -K2 * vfi_P[0][1] + vfi_P[1][1];
const int32_t P23 = -K2 * vfi_P[0][2] + vfi_P[1][2];
const int32_t P31 = -K3 * vfi_P[0][0] + vfi_P[2][0];
const int32_t P32 = -K3 * vfi_P[0][1] + vfi_P[2][1];
const int32_t P33 = -K3 * vfi_P[0][2] + vfi_P[2][2];
tl_vf_P[0][0] = P11;
tl_vf_P[0][1] = P12;
sw/airborne/firmwares/rotorcraft/ins/vf_int.h
+20 -20
View File
@@ -21,8 +21,8 @@
* Boston, MA 02111-1307, USA.
*/
#ifndef BOOZ2_VF_INT_H
#define BOOZ2_VF_INT_H
#ifndef VF_INT_H
#define VF_INT_H
#include "std.h"
#include "booz_geometry_int.h"
@@ -33,43 +33,43 @@ extern void vfi_propagate( int32_t accel_reading );
/* z_meas : altitude measurement in meter */
/* Q23.8 : accuracy 0.004m range 8388km */
extern void vfi_update( int32_t z_meas );
#define B2_VFI_Z_MEAS_FRAC IPOS_FRAC
#define VFI_Z_MEAS_FRAC IPOS_FRAC
/* propagate frequency : 512 Hz */
#define B2_VFI_F_UPDATE_FRAC 9
#define B2_VFI_F_UPDATE (1<<B2_VFI_F_UPDATE_RES)
#define VFI_F_UPDATE_FRAC 9
#define VFI_F_UPDATE (1<<VFI_F_UPDATE_RES)
/* vertical acceleration in m/s^2 */
/* Q21.10 : accuracy 0.001m/s^2, range 2097km/s2 */
extern int32_t b2_vfi_zdd;
#define B2_VFI_ZDD_FRAC IACCEL_RES
extern int32_t vfi_zdd;
#define VFI_ZDD_FRAC IACCEL_RES
/* vertical accelerometer bias in m/s^2 */
/* Q21.10 : accuracy 0.001m/s^2, range 2097km/s2 */
extern int32_t b2_vfi_abias;
#define B2_VFI_BIAS_FRAC IACCEL_RES
extern int32_t vfi_abias;
#define VFI_BIAS_FRAC IACCEL_RES
/* vertical speed in m/s */
/* Q12.19 : accuracy 0.000002 , range 4096m/s2 */
extern int32_t b2_vfi_zd;
#define B2_VFI_ZD_FRAC (B2_VFI_ZDD_FRAC + B2_VFI_F_UPDATE_FRAC)
extern int32_t vfi_zd;
#define VFI_ZD_FRAC (VFI_ZDD_FRAC + VFI_F_UPDATE_FRAC)
/* altitude in m */
/* Q35.28 : accuracy 3.7e-9 , range 3.4e10m */
extern int64_t b2_vfi_z;
#define B2_VFI_Z_FRAC (B2_VFI_ZD_FRAC + B2_VFI_F_UPDATE_FRAC)
extern int64_t vfi_z;
#define VFI_Z_FRAC (VFI_ZD_FRAC + VFI_F_UPDATE_FRAC)
/* Kalman filter state */
#define B2_VFI_S_Z 0
#define B2_VFI_S_ZD 1
#define B2_VFI_S_AB 2
#define B2_VFI_S_SIZE 3
#define VFI_S_Z 0
#define VFI_S_ZD 1
#define VFI_S_AB 2
#define VFI_S_SIZE 3
/* Kalman filter covariance */
/* Q3.28 */
extern int32_t b2_vfi_P[B2_VFI_S_SIZE][B2_VFI_S_SIZE];
#define B2_VFI_P_FRAC 28
extern int32_t vfi_P[VFI_S_SIZE][VFI_S_SIZE];
#define VFI_P_FRAC 28
#endif /* BOOZ2_VF_INT_H */
#endif /* VF_INT_H */