/* 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 }