Paparazzi UAS  v4.0.4_stable-3-gf39211a
Paparazzi is a free software Unmanned Aircraft System.
 All Data Structures Files Functions Variables Typedefs Enumerations Enumerator Macros Pages
vf_int.c
Go to the documentation of this file.
1 /*
2  * $Id$
3  *
4  * Copyright (C) 2008-2009 Antoine Drouin <poinix@gmail.com>
5  *
6  * This file is part of paparazzi.
7  *
8  * paparazzi is free software; you can redistribute it and/or modify
9  * it under the terms of the GNU General Public License as published by
10  * the Free Software Foundation; either version 2, or (at your option)
11  * any later version.
12  *
13  * paparazzi is distributed in the hope that it will be useful,
14  * but WITHOUT ANY WARRANTY; without even the implied warranty of
15  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
16  * GNU General Public License for more details.
17  *
18  * You should have received a copy of the GNU General Public License
19  * along with paparazzi; see the file COPYING. If not, write to
20  * the Free Software Foundation, 59 Temple Place - Suite 330,
21  * Boston, MA 02111-1307, USA.
22  */
23 
24 #include "subsystems/ins/vf_int.h"
25 
26 #include "booz_geometry_mixed.h"
27 
33 
34 /* initial covariance */
35 #define VFI_INIT_PZZ BOOZ_INT_OF_FLOAT(1., VFI_P_FRAC)
36 #define VFI_INIT_PZDZD BOOZ_INT_OF_FLOAT(1., VFI_P_FRAC)
37 #define VFI_INIT_PABAB BOOZ_INT_OF_FLOAT(1., VFI_P_FRAC)
38 
39 /* system and measurement noise */
40 #define VFI_ACCEL_NOISE 0.1
41 #define VFI_DT2_2 (1./(512.*512.)/2.)
42 #define VFI_DT (1./512.)
43 #define VFI_QZZ BOOZ_INT_OF_FLOAT(VFI_ACCEL_NOISE*VFI_DT2_2, VFI_P_FRAC)
44 #define VFI_QZDZD BOOZ_INT_OF_FLOAT(VFI_ACCEL_NOISE*VFI_DT, VFI_P_FRAC)
45 #define VFI_QABAB BOOZ_INT_OF_FLOAT(1e-7, VFI_P_FRAC)
46 #define VFI_R BOOZ_INT_OF_FLOAT(1., VFI_P_FRAC)
47 
48 
49 void vfi_init(int32_t z0, int32_t zd0, int32_t bias0 ) {
50 
51  // initialize state vector
52  vfi_z = z0;
53  vfi_zd = zd0;
54  vfi_abias = bias0;
55  vfi_zdd = 0;
56  // initialize covariance
57  int i, j;
58  for (i=0; i<VFI_S_SIZE; i++)
59  for (j=0; j<VFI_S_SIZE; j++)
60  vfi_P[i][j] = 0;
64 
65 }
66 
67 /*
68 
69  F = [ 1 dt -dt^2/2
70  0 1 -dt
71  0 0 1 ];
72 
73  B = [ dt^2/2 dt 0]';
74 
75  Q = [ 0.01 0 0
76  0 0.01 0
77  0 0 0.001 ];
78 
79  Xk1 = F * Xk0 + B * accel;
80 
81  Pk1 = F * Pk0 * F' + Q;
82 
83 */
84 
85 void vfi_propagate( int32_t accel_reading ) {
86 
87  // compute unbiased vertical acceleration
88  vfi_zdd = accel_reading + BOOZ_INT_OF_FLOAT(9.81, VFI_ZDD_FRAC) - vfi_abias;
89  // propagate state
91  vfi_z += dz;
93  vfi_zd += dzd;
94 
95  // propagate covariance
96  const int32_t tmp1 = vfi_P[1][0] + vfi_P[0][1] + (vfi_P[1][1]>>VFI_F_UPDATE_FRAC);
97  const int32_t FPF00 = vfi_P[0][0] + (tmp1>>VFI_F_UPDATE_FRAC);
98  const int32_t tmp2 = vfi_P[1][1] - vfi_P[0][2] - (vfi_P[1][2]>>VFI_F_UPDATE_FRAC);
99  const int32_t FPF01 = vfi_P[0][1] + (tmp2>>VFI_F_UPDATE_FRAC);
100  const int32_t FPF02 = vfi_P[0][2] + (vfi_P[1][2] >> VFI_F_UPDATE_FRAC);;
101  const int32_t tmp3 = -vfi_P[2][0] + vfi_P[1][1] - (vfi_P[2][1]>>VFI_F_UPDATE_FRAC);
102  const int32_t FPF10 = vfi_P[1][0] + (tmp3>>VFI_F_UPDATE_FRAC);
103  const int32_t tmp4 = -vfi_P[2][1] - vfi_P[1][2] + (vfi_P[2][2]>>VFI_F_UPDATE_FRAC);
104  const int32_t FPF11 = vfi_P[1][1] + (tmp4>>VFI_F_UPDATE_FRAC);
105  const int32_t FPF12 = vfi_P[1][2] - (vfi_P[2][2] >> VFI_F_UPDATE_FRAC);
106  const int32_t FPF20 = vfi_P[2][0] + (vfi_P[2][1] >> VFI_F_UPDATE_FRAC);
107  const int32_t FPF21 = vfi_P[2][1] - (vfi_P[2][2] >> VFI_F_UPDATE_FRAC);
108  const int32_t FPF22 = vfi_P[2][2];
109 
110  vfi_P[0][0] = FPF00 + VFI_QZZ;
111  vfi_P[0][1] = FPF01;
112  vfi_P[0][2] = FPF02;
113  vfi_P[1][0] = FPF10;
114  vfi_P[1][1] = FPF11 + VFI_QZDZD;
115  vfi_P[1][2] = FPF12;
116  vfi_P[2][0] = FPF20;
117  vfi_P[2][1] = FPF21;
118  vfi_P[2][2] = FPF22 + VFI_QABAB;
119 
120 }
121 
122 
123 void vfi_update( int32_t z_meas ) {
124 
125  const int64_t y = (z_meas<<(VFI_Z_FRAC-VFI_MEAS_Z_FRAC)) - vfi_z;
126  const int32_t S = vfi_P[0][0] + VFI_R;
127 
128  const int32_t K1 = vfi_P[0][0] / S;
129  const int32_t K2 = vfi_P[1][0] / S;
130  const int32_t K3 = vfi_P[2][0] / S;
131 
132  vfi_z = vfi_z + ((K1 * y)>>VFI_P_FRAC);
133  vfi_zd = vfi_zd + ((K2 * y)>>VFI_P_FRAC);
134  vfi_abias = vfi_abias + ((K3 * y)>>VFI_P_FRAC);
135 
136 #if 0
137 
138  const int32_t P11 = ((BOOZ_INT_OF_FLOAT(1., VFI_P_RES) - K1) * vfi_P[0][0])>>VFI_P_RES;
139  const int32_t P12 = (BOOZ_INT_OF_FLOAT(1., VFI_P_RES) - K1) * vfi_P[0][1];
140  const int32_t P13 = (BOOZ_INT_OF_FLOAT(1., VFI_P_RES) - K1) * vfi_P[0][2];
141  const int32_t P21 = -K2 * vfi_P[0][0] + vfi_P[1][0];
142  const int32_t P22 = -K2 * vfi_P[0][1] + vfi_P[1][1];
143  const int32_t P23 = -K2 * vfi_P[0][2] + vfi_P[1][2];
144  const int32_t P31 = -K3 * vfi_P[0][0] + vfi_P[2][0];
145  const int32_t P32 = -K3 * vfi_P[0][1] + vfi_P[2][1];
146  const int32_t P33 = -K3 * vfi_P[0][2] + vfi_P[2][2];
147 
148  tl_vf_P[0][0] = P11;
149  tl_vf_P[0][1] = P12;
150  tl_vf_P[0][2] = P13;
151  tl_vf_P[1][0] = P21;
152  tl_vf_P[1][1] = P22;
153  tl_vf_P[1][2] = P23;
154  tl_vf_P[2][0] = P31;
155  tl_vf_P[2][1] = P32;
156  tl_vf_P[2][2] = P33;
157 #endif
158 }
#define VFI_QZZ
Definition: vf_int.c:43
void vfi_update(int32_t z_meas)
Definition: vf_int.c:123
int32_t vfi_P[VFI_S_SIZE][VFI_S_SIZE]
Definition: vf_int.c:32
#define VFI_INIT_PABAB
Definition: vf_int.c:37
#define VFI_R
Definition: vf_int.c:46
signed long long int64_t
Definition: types.h:21
void vfi_init(int32_t z0, int32_t zd0, int32_t bias0)
Definition: vf_int.c:49
#define VFI_INIT_PZDZD
Definition: vf_int.c:36
#define VFI_QABAB
Definition: vf_int.c:45
#define VFI_F_UPDATE_FRAC
Definition: vf_int.h:39
#define VFI_Z_FRAC
Definition: vf_int.h:60
#define VFI_P_FRAC
Definition: vf_int.h:70
#define VFI_S_Z
Definition: vf_int.h:63
int32_t vfi_abias
Definition: vf_int.c:30
#define VFI_INIT_PZZ
Definition: vf_int.c:35
void vfi_propagate(int32_t accel_reading)
Definition: vf_int.c:85
int32_t vfi_zd
Definition: vf_int.c:29
#define VFI_ZD_FRAC
Definition: vf_int.h:55
signed long int32_t
Definition: types.h:19
#define VFI_ZDD_FRAC
Definition: vf_int.h:45
int32_t vfi_zdd
Definition: vf_int.c:31
#define VFI_S_AB
Definition: vf_int.h:65
#define VFI_S_ZD
Definition: vf_int.h:64
float y
Definition: hf_float.h:57
#define VFI_QZDZD
Definition: vf_int.c:44
#define VFI_S_SIZE
Definition: vf_int.h:66
int64_t vfi_z
Definition: vf_int.c:28