A base class for (Extended) Kalman Filter

KalmanFilter.py

from typing import Tuple
import numpy as np

class BaseKF:
    """This class can hold either a Kalman filter or an extended kalman filter depending on 
    the way f and h are implemented.
    """
    def __init__(self, z0: np.array, r: np.array, q: np.array, pval=0.1) -> None:
        self.x_sk = z0
        self.n = q.shape[0]  # number of state
        self.m = r.shape[0]  # number of sensors
        
        # print(self.n)
        self.Pk = np.eye(self.n) * pval
        self.Gk = None

        self.R = np.eye(self.m) * r  # Noise of sensor
        self.Q = np.eye(self.n) * q  # Covariance of process
        

        self.predictions = [self.x_sk]

        super().__init__()

    def f(self, x, u) -> Tuple[np.array, np.array]:
        """M is the number of state values
           N is the number of sensor inputs
           state model and transition function

        Args:
            x (np.array): the inputs of size M
        
        In the Kalman filter we use the model matrix A only and this function returns the predicted state estimation
        x---x = A*x + B*u---and the model A.
        If this function returns the newly estimated state and the model itself it's a Linear Kalman Filter.
        
        In the Extended Kalman filter, we need the jacobian F of the state-transition function f---x = f(x, u).
        If this function returns the newly estimated state and jacobian it's a Extended Kalman Filter.
            
        Returns:
            Tuple[np.array, np.array]: return x an array of size M and 
            either its jacobian F---or the model A---an arrayx of size MxN.
            
        """
    
    def h(self, x) -> Tuple[np.array, np.array]:
        """obersvation model and obersvation function
        
        In the kalman filter the observation z_e is estimared using C.x_e.
        If the method returns the estimated observation--C.x_e---and the model C it's a Linear Kalman Filter.
        
        In the Extended Kalman filter, the observation z_e is estimated
        using a linear function h---z_e = h(x)---and H is the jacobian of h.
        If the method returns the estimated observation---h(x)---and the jacobian H it's a Extended Kalman Filter.

        Returns:
            Tuple[np.array, np.array]: retur h a array of size M and 
            either its jacobian H---or the model C---an arrayx of size MxN.
           
        """
        pass

    def predict(self, u):
        self.x_sk, F = self.f(self.x_sk, u)
        self.Pk = (F * self.Pk * np.transpose(F)) + self.Q
    
    def update(self, z):
        """update step

        Args:
            z (np.array): z is an array of size M
        """
        h, Hk = self.h(self.x_sk)
        inverse_array = np.linalg.inv(np.matmul(Hk, np.matmul(self.Pk, np.transpose(Hk)) + self.R))
        self.Gk = np.matmul(np.matmul(self.Pk, np.transpose(Hk)), inverse_array)
        self.x_sk = self.x_sk + np.matmul(self.Gk, (np.array(z) - h))
        self.Pk = np.matmul(np.eye(self.n) - np.matmul(self.Gk, Hk), self.Pk)
        
        self.predictions.append(self.x_sk)