Added diagonal vector configuration for initial_covariance and process_noise
Fixed potential issue with gravity.