/*
F = [ 1 dt -dt^2/2 0
0 1 -dt 0
0 0 1 0
0 0 0 1 ];
B = [ dt^2/2 dt 0 0]';
Q = [ Qzz 0 0 0
0 Qzdotzdot 0 0
0 0 Qbiasbias 0
0 0 0 0 Qoffoff ];
Xk1 = F * Xk0 + B * accel;
Pk1 = F * Pk0 * F' + Q;
*/
void vff_propagate(float accel) {
/* update state */
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 = 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];
const float FPF33 = vff_P[3][3];
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;
vff_P[3][3] = FPF33 + Qoffoff;
#if DEBUG_VFF_EXTENDED
RunOnceEvery(10, send_vffe());
#endif
}
/*
F = [ 1 dt -dt^2/2 0
0 1 -dt 0
0 0 1 0
0 0 0 1 ];
B = [ dt^2/2 dt 0 0]';
Q = [ Qzz 0 0 0
0 Qzdotzdot 0 0
0 0 Qbiasbias 0
0 0 0 0 Qoffoff ];
Qzz = ACCEL_NOISE * DT_VFILTER * DT_VFILTER / 2.
Qzdotzdot = ACCEL_NOISE * DT_VFILTER
Xk1 = F * Xk0 + B * accel;
Pk1 = F * Pk0 * F' + Q;
*/
void vff_propagate(float accel, float dt)
{
/* update state */
vff.zdotdot = accel + 9.81 - vff.bias;
vff.z = vff.z + dt * vff.zdot;
vff.zdot = vff.zdot + dt * vff.zdotdot;
/* update covariance */
const float FPF00 = vff.P[0][0] + dt * (vff.P[1][0] + vff.P[0][1] + dt * vff.P[1][1]);
const float FPF01 = vff.P[0][1] + dt * (vff.P[1][1] - vff.P[0][2] - dt * vff.P[1][2]);
const float FPF02 = vff.P[0][2] + dt * (vff.P[1][2]);
const float FPF10 = vff.P[1][0] + dt * (-vff.P[2][0] + vff.P[1][1] - dt * vff.P[2][1]);
const float FPF11 = vff.P[1][1] + dt * (-vff.P[2][1] - vff.P[1][2] + dt * vff.P[2][2]);
const float FPF12 = vff.P[1][2] + dt * (-vff.P[2][2]);
const float FPF20 = vff.P[2][0] + dt * (vff.P[2][1]);
const float FPF21 = vff.P[2][1] + dt * (-vff.P[2][2]);
const float FPF22 = vff.P[2][2];
const float FPF33 = vff.P[3][3];
vff.P[0][0] = FPF00 + ACCEL_NOISE * dt * dt / 2.;
vff.P[0][1] = FPF01;
vff.P[0][2] = FPF02;
vff.P[1][0] = FPF10;
vff.P[1][1] = FPF11 + ACCEL_NOISE * dt;
vff.P[1][2] = FPF12;
vff.P[2][0] = FPF20;
vff.P[2][1] = FPF21;
vff.P[2][2] = FPF22 + Qbiasbias;
vff.P[3][3] = FPF33 + Qoffoff;
#if DEBUG_VFF_EXTENDED
RunOnceEvery(10, send_vffe(&(DefaultChannel).trans_tx, &(DefaultDevice).device));
#endif
}
/**
* Propagate the filter in time.
*
* F = [ 1 dt -dt^2/2 0 0
* 0 1 -dt 0 0
* 0 0 1 0 0
* 0 0 0 1 0
* 0 0 0 0 1 ];
*
* B = [ dt^2/2 dt 0 0 0]';
*
* Q = [ Qzz 0 0 0 0
* 0 Qzdotzdot 0 0 0
* 0 0 Qbiasbias 0 0
* 0 0 0 0 Qoffoff 0
* 0 0 0 0 0 Qobsobs ];
*
* Qzz = VFF_EXTENDED_ACCEL_NOISE * DT_VFILTER * DT_VFILTER / 2.
* Qzdotzdot = VFF_EXTENDED_ACCEL_NOISE * DT_VFILTER
*
* Xk1 = F * Xk0 + B * accel;
*
* Pk1 = F * Pk0 * F' + Q;
*/
void vff_propagate(float accel, float dt)
{
/* update state */
vff.zdotdot = accel + 9.81f - vff.bias;
vff.z += (vff.zdot + vff.zdotdot * dt) * dt / 2.f; // trapizium integration
vff.zdot += vff.zdotdot * dt;
/* update covariance */
const float FPF00 = vff.P[0][0] + dt * (vff.P[1][0] + vff.P[0][1] + dt * vff.P[1][1]);
const float FPF01 = vff.P[0][1] + dt * (vff.P[1][1] - vff.P[0][2] - dt * vff.P[1][2]);
const float FPF02 = vff.P[0][2] + dt * (vff.P[1][2]);
const float FPF10 = vff.P[1][0] + dt * (-vff.P[2][0] + vff.P[1][1] - dt * vff.P[2][1]);
const float FPF11 = vff.P[1][1] + dt * (-vff.P[2][1] - vff.P[1][2] + dt * vff.P[2][2]);
const float FPF12 = vff.P[1][2] + dt * (-vff.P[2][2]);
const float FPF20 = vff.P[2][0] + dt * (vff.P[2][1]);
const float FPF21 = vff.P[2][1] + dt * (-vff.P[2][2]);
const float FPF22 = vff.P[2][2];
const float FPF33 = vff.P[3][3];
const float FPF44 = vff.P[4][4];
vff.P[0][0] = FPF00 + vff.accel_noise * dt * dt / 2.f;
vff.P[0][1] = FPF01;
vff.P[0][2] = FPF02;
vff.P[1][0] = FPF10;
vff.P[1][1] = FPF11 + vff.accel_noise * dt;
vff.P[1][2] = FPF12;
vff.P[2][0] = FPF20;
vff.P[2][1] = FPF21;
vff.P[2][2] = FPF22 + Qbiasbias;
vff.P[3][3] = FPF33 + Qoffoff;
vff.P[4][4] = FPF44 + Qobsobs;
#if DEBUG_VFF_EXTENDED
RunOnceEvery(10, send_vffe(&(DefaultChannel).trans_tx, &(DefaultDevice).device));
#endif
#if DEBUG_VFF_EXTENDED > 1
RunOnceEvery(100, print_vff());
#endif
}
