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