Update KalmanForWaveAlt.h

bbn_wave_freq_m5atomS3/KalmanForWaveAlt.h

@@ -108,7 +108,7 @@ matrix_t *kalman_wave_alt_get_state_transition(kalman_t *kf, matrix_data_t k_hat
   matrix_set(F, 1, 2, (matrix_data_t)delta_t);                     // T
   matrix_set(F, 1, 3, (matrix_data_t)-delta_t);                    // -T
 
-  matrix_set(F, 2, 0, (matrix_data_t)k_hat);                // k
+  matrix_set(F, 2, 0, (matrix_data_t)k_hat);            // k_hat
   matrix_set(F, 2, 1, (matrix_data_t)0.0);              // 0
   matrix_set(F, 2, 2, (matrix_data_t)0.0);              // 0
   matrix_set(F, 2, 3, (matrix_data_t)0.0);              // 0
@@ -171,16 +171,6 @@ void kalman_wave_alt_init_defaults() {
   matrix_set_symmetric(Q, 3, 3, (matrix_data_t)0.008 * variance);
 }
 
-matrix_t *kalman_wave_alt_get_transition_offset(kalman_t *kf, matrix_data_t delta_t) {
-  // transition offset [KALMAN_NUM_STATES * KALMAN_NUM_INPUTS]
-  matrix_t *B = kalman_get_input_transition(kf);
-  matrix_set(B, 0, 0, (matrix_data_t)0.0);
-  matrix_set(B, 1, 0, (matrix_data_t)0.0);
-  matrix_set(B, 2, 0, (matrix_data_t)0.0);
-  matrix_set(B, 3, 0, (matrix_data_t)0.0);
-  return B;
-}
-
 void kalman_wave_alt_step(KalmanWaveAltState* state, float accel, float k_hat, float delta_t) {
   kalman_t *kf = &kalman_filter_wave_alt;
   kalman_measurement_t *kfm = &kalman_filter_wave_alt_measurement_vert_accel;
@@ -189,11 +179,6 @@ void kalman_wave_alt_step(KalmanWaveAltState* state, float accel, float k_hat, f
   matrix_t *z = kalman_get_measurement_vector(kfm);
 
   matrix_t *F = kalman_wave_alt_get_state_transition(kf, k_hat, delta_t);
-  matrix_t *B = kalman_wave_alt_get_transition_offset(kf, delta_t);
-
-  // input vector [KALMAN_NUM_INPUTS * 1]
-  matrix_t *u = kalman_get_input_vector(kf);
-  matrix_set(u, 0, 0, 0.0);
 
   // prediction.
   kalman_predict(kf);