In: observation: Nx3 Out: pred_state: Nx3
(observations,damping=1)
| 348 | return pred_state |
| 349 | |
| 350 | def Kalman3D(observations,damping=1): |
| 351 | ''' |
| 352 | In: |
| 353 | observation: Nx3 |
| 354 | Out: |
| 355 | pred_state: Nx3 |
| 356 | ''' |
| 357 | # To return the smoothed time series data |
| 358 | observation_covariance = damping |
| 359 | transition_matrix = 1 |
| 360 | transition_covariance = 0.1 |
| 361 | initial_value_guess_x = observations[0,0] |
| 362 | initial_value_guess_y = observations[0,1] # ? |
| 363 | initial_value_guess_z = observations[0,2] # ? |
| 364 | |
| 365 | # perform 1D smooth for each axis |
| 366 | kfx = KalmanFilter( |
| 367 | initial_state_mean=initial_value_guess_x, |
| 368 | initial_state_covariance=observation_covariance, |
| 369 | observation_covariance=observation_covariance, |
| 370 | transition_covariance=transition_covariance, |
| 371 | transition_matrices=transition_matrix |
| 372 | ) |
| 373 | pred_state_x, state_cov_x = kfx.smooth(observations[:, 0]) |
| 374 | |
| 375 | kfy = KalmanFilter( |
| 376 | initial_state_mean=initial_value_guess_y, |
| 377 | initial_state_covariance=observation_covariance, |
| 378 | observation_covariance=observation_covariance, |
| 379 | transition_covariance=transition_covariance, |
| 380 | transition_matrices=transition_matrix |
| 381 | ) |
| 382 | pred_state_y, state_cov_y = kfy.smooth(observations[:, 1]) |
| 383 | |
| 384 | kfz = KalmanFilter( |
| 385 | initial_state_mean=initial_value_guess_z, |
| 386 | initial_state_covariance=observation_covariance, |
| 387 | observation_covariance=observation_covariance, |
| 388 | transition_covariance=transition_covariance, |
| 389 | transition_matrices=transition_matrix |
| 390 | ) |
| 391 | pred_state_z, state_cov_z = kfy.smooth(observations[:, 2]) |
| 392 | |
| 393 | pred_state = np.concatenate((pred_state_x, pred_state_y, pred_state_z), axis=1) |
| 394 | return pred_state |
nothing calls this directly
no outgoing calls
no test coverage detected