xylambda / kalmankit Goto Github PK

def smooth(Z, U):
      # allow U to be None without the filter failing
      U = check_none_and_broadcast(U, Z)

      # filtering process to get posteriors
      x_est, P_est = self.filter(Z=Z, U=U)

      # mean and covariance array allocation
      xk_smooth = np.zeros((len(Z), self.state_size))
      Pk_smooth = np.zeros((len(Z), self.state_size, self.state_size))

      # smooth initialization
      xk_smooth[-1] = x_est[-1]
      Pk_smooth[-1] = P_est[-1]

      n_obs = len(Z)
      for k in range(n_obs - 2, -1, -1):
          # select appropiate parameters for each time step
          xk = x_est[k]
          uk = U[k]
          Qk = self.Q[k]

          # predicted mean and covariance
          xk_ahead = self.f(xk, uk)  # mk is x_est[k]
          Ak = self.jacobian_A(x_est[k], uk)
          Pk_ahead = Ak @ (P_est[k] @ Ak.T) + Qk

          # smooth (like butter) process
          Kk = P_est[k] @ (Ak.T @ np.linalg.pinv(Pk_ahead))
          xk_smooth[k] = x_est[k] + Kk @ (xk_smooth[k + 1] - xk_ahead)
          Pk_smooth[k] = P_est[k] + Kk @ ((Pk_smooth[k + 1] - Pk_ahead.T) @ Kk)

      return xk_smooth, Pk_smooth