Update KalmanForWave.h

bareboat-necessities · Sep 7, 2024 · 3d203f1 · 3d203f1
1 parent 35dea88
commit 3d203f1
Showing 1 changed file with 6 additions and 8 deletions.
diff --git a/bbn_wave_freq_m5atomS3/KalmanForWave.h b/bbn_wave_freq_m5atomS3/KalmanForWave.h
@@ -35,12 +35,12 @@ matrix_t *kalman_wave_get_state_transition(kalman_t *kf, matrix_data_t delta_t)
   return F;
 }
 
-matrix_t *kalman_wave_get_transition_offset(kalman_t *kf, matrix_data_t delta_t) {
+matrix_t *kalman_wave_get_transition_offset(kalman_t *kf, matrix_data_t delta_t, matrix_data_t accel) {
   // transition offset [KALMAN_NUM_STATES * KALMAN_NUM_INPUTS]
   matrix_t *B = kalman_get_input_transition(kf);
-  matrix_set(B, 0, 0, (matrix_data_t)(1.0 / 6.0) * delta_t * delta_t * delta_t);
-  matrix_set(B, 1, 0, (matrix_data_t)0.5 * delta_t * delta_t);
-  matrix_set(B, 2, 0, (matrix_data_t)delta_t);
+  matrix_set(B, 0, 0, (matrix_data_t)(1.0 / 6.0) * delta_t * delta_t * delta_t * accel);
+  matrix_set(B, 1, 0, (matrix_data_t)0.5 * delta_t * delta_t * accel);
+  matrix_set(B, 2, 0, (matrix_data_t)delta_t * accel);
   return B;
 }
 
@@ -84,17 +84,15 @@ void kalman_wave_init() {
   matrix_set_symmetric(Q, 2, 2, (matrix_data_t)0.1);
 }
 
-void kalman_wave_update(float accel, float delta_t) {
+void kalman_wave_step(float accel, float delta_t) {
   kalman_t *kf = &kalman_filter_wave;
   kalman_measurement_t *kfm = &kalman_filter_wave_measurement_displacement_integral;
 
   matrix_t *x = kalman_get_state_vector(kf);
   matrix_t *z = kalman_get_measurement_vector(kfm);
 
   matrix_t *F = kalman_wave_get_state_transition(kf, delta_t);
-  matrix_t *B = kalman_wave_get_transition_offset(kf, delta_t);
-
-  // TODO: B = B * accel ? 
+  matrix_t *B = kalman_wave_get_transition_offset(kf, delta_t, accel);
 
   // prediction.
   kalman_predict(kf);