From 0d0de2fd3193e4cf61a2b0b44c88d909bb6672b7 Mon Sep 17 00:00:00 2001 From: Gerardo Duran-Martin Date: Wed, 4 May 2022 00:27:14 +0000 Subject: [PATCH 1/2] fix: kalman_filter.py Fix update mu step --- jsl/lds/kalman_filter.py | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/jsl/lds/kalman_filter.py b/jsl/lds/kalman_filter.py index db9c9dc..8409984 100644 --- a/jsl/lds/kalman_filter.py +++ b/jsl/lds/kalman_filter.py @@ -2,7 +2,7 @@ # Author: Gerardo Durán-Martín (@gerdm), Aleyna Kara(@karalleyna), Kevin Murphy (@murphyk) from jax import config -config.update('jax_default_matmul_precision', 'float32') +config.update("jax_default_matmul_precision", "float32") import chex import jax.numpy as jnp @@ -202,7 +202,7 @@ def kalman_step(state, obs, params): innovation = Ct @ mu_cond #innovation = innovation + params.get_obs_offset_of(t) - mu = mu_cond + Kt @ innovation + mu = mu_cond + Kt @ (obs - innovation) # More stable solution is (I − KtCt)Σt|t−1(I − KtCt)T + KtRtKTt tmp = (I - Kt @ Ct) From b50ac8103307ef7fa54dfe9f8c979744a67c131e Mon Sep 17 00:00:00 2001 From: Gerardo Duran-Martin Date: Wed, 4 May 2022 00:35:52 +0000 Subject: [PATCH 2/2] feat: kalman_filter.py Add offset term --- jsl/lds/kalman_filter.py | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/jsl/lds/kalman_filter.py b/jsl/lds/kalman_filter.py index 8409984..5ad4890 100644 --- a/jsl/lds/kalman_filter.py +++ b/jsl/lds/kalman_filter.py @@ -192,7 +192,7 @@ def kalman_step(state, obs, params): # \mu_{t |t-1} and xn|{n-1} mu_cond = A @ mu - #mu_cond = mu_cond + params.get_state_offset_of(t) + mu_cond = mu_cond + params.get_state_offset_of(t) Ct = params.get_obs_mat_of(t) R = params.get_observation_noise_of(t) @@ -201,7 +201,7 @@ def kalman_step(state, obs, params): Kt = solve(St, Ct @ Sigma_cond, sym_pos=True).T innovation = Ct @ mu_cond - #innovation = innovation + params.get_obs_offset_of(t) + innovation = innovation + params.get_obs_offset_of(t) mu = mu_cond + Kt @ (obs - innovation) # More stable solution is (I − KtCt)Σt|t−1(I − KtCt)T + KtRtKTt