32#include "generated/airframe.h"
37#define VFF_INIT_PXX 1.0
41#ifndef VFF_ACCEL_NOISE
42#define VFF_ACCEL_NOISE 0.5
47#define VFF_MEAS_NOISE 1.0
58static void send_vff(
struct transport_tx *trans,
struct link_device *
dev)
60 pprz_msg_send_VFF(trans,
dev, AC_ID,
62 &
vff.P[0][0], &
vff.P[1][1], &
vff.P[2][2]);
71void vff_init(
float init_z,
float init_zdot,
float init_bias)
111 vff.zdotdot = accel + 9.81 -
vff.bias;
115 const float FPF00 =
vff.P[0][0] + dt * (
vff.P[1][0] +
vff.P[0][1] + dt *
vff.P[1][1]);
116 const float FPF01 =
vff.P[0][1] + dt * (
vff.P[1][1] -
vff.P[0][2] - dt *
vff.P[1][2]);
117 const float FPF02 =
vff.P[0][2] + dt * (
vff.P[1][2]);
118 const float FPF10 =
vff.P[1][0] + dt * (-
vff.P[2][0] +
vff.P[1][1] - dt *
vff.P[2][1]);
119 const float FPF11 =
vff.P[1][1] + dt * (-
vff.P[2][1] -
vff.P[1][2] + dt *
vff.P[2][2]);
120 const float FPF12 =
vff.P[1][2] + dt * (-
vff.P[2][2]);
121 const float FPF20 =
vff.P[2][0] + dt * (
vff.P[2][1]);
122 const float FPF21 =
vff.P[2][1] + dt * (-
vff.P[2][2]);
123 const float FPF22 =
vff.P[2][2];
158 const float S =
vff.P[0][0] + conf;
159 const float K1 =
vff.P[0][0] * 1 / S;
160 const float K2 =
vff.P[1][0] * 1 / S;
161 const float K3 =
vff.P[2][0] * 1 / S;
164 vff.zdot =
vff.zdot + K2 * y;
165 vff.bias =
vff.bias + K3 * y;
167 const float P11 = (1. - K1) *
vff.P[0][0];
168 const float P12 = (1. - K1) *
vff.P[0][1];
169 const float P13 = (1. - K1) *
vff.P[0][2];
170 const float P21 = -K2 *
vff.P[0][0] +
vff.P[1][0];
171 const float P22 = -K2 *
vff.P[0][1] +
vff.P[1][1];
172 const float P23 = -K2 *
vff.P[0][2] +
vff.P[1][2];
173 const float P31 = -K3 *
vff.P[0][0] +
vff.P[2][0];
174 const float P32 = -K3 *
vff.P[0][1] +
vff.P[2][1];
175 const float P33 = -K3 *
vff.P[0][2] +
vff.P[2][2];
196 if (conf < 0.f) {
return; }
217 const float yd = vz -
vff.zdot;
218 const float S =
vff.P[1][1] + conf;
219 const float K1 =
vff.P[0][1] * 1 / S;
220 const float K2 =
vff.P[1][1] * 1 / S;
221 const float K3 =
vff.P[2][1] * 1 / S;
224 vff.zdot =
vff.zdot + K2 * yd;
225 vff.bias =
vff.bias + K3 * yd;
227 const float P11 = -K1 *
vff.P[1][0] +
vff.P[0][0];
228 const float P12 = -K1 *
vff.P[1][1] +
vff.P[0][1];
229 const float P13 = -K1 *
vff.P[1][2] +
vff.P[0][2];
230 const float P21 = (1. - K2) *
vff.P[1][0];
231 const float P22 = (1. - K2) *
vff.P[1][1];
232 const float P23 = (1. - K2) *
vff.P[1][2];
233 const float P31 = -K3 *
vff.P[1][0] +
vff.P[2][0];
234 const float P32 = -K3 *
vff.P[1][1] +
vff.P[2][1];
235 const float P33 = -K3 *
vff.P[1][2] +
vff.P[2][2];
251 if (conf < 0.f) {
return; }
static struct uart_periph * dev
int16_t register_periodic_telemetry(struct periodic_telemetry *_pt, uint16_t _id, telemetry_cb _cb)
Register a telemetry callback function.
Periodic telemetry system header (includes downlink utility and generated code).
#define DefaultPeriodic
Set default periodic telemetry.
void vff_update(float z_meas)
static void update_vz_conf(float vz, float conf)
void vff_init(float init_z, float init_zdot, float init_bias)
void vff_update_z_conf(float z_meas, float conf)
static void send_vff(struct transport_tx *trans, struct link_device *dev)
void vff_update_vz_conf(float vz_meas, float conf)
void vff_realign(float z_meas)
void vff_propagate(float accel, float dt)
Propagate the filter in time.
#define VFF_MEAS_NOISE
measurement noise covariance R
static void update_z_conf(float z_meas, float conf)
Update altitude.
#define VFF_ACCEL_NOISE
process noise covariance Q
#define VFF_INIT_PXX
initial error covariance diagonal
Vertical filter (in float) estimating altitude, velocity and accel bias.
float z_meas
last measurement