import numpy as np

class KalmanFilter(object):
    def __init__(self, x, P, F, Q, H, R, ticksPerSecond):
        self.tps = ticksPerSecond
        dt = 1/ticksPerSecond
        
        # Zustandsvektor
        self.x = x

        # Kovarianzmatrix (hohe Werte fuer schnelle Aenderungen)
        self.P = P
        self.originP = P


        # Dynamikmatrix
        self.F = F

        # Prozessrausch-Matrix (Kovarianzmatrix)
        self.Q = Q

        # Messmatrix um die Messung auf die relavanten Daten zu reduzieren
        self.H = H

        # Messrausch-Kovarianzmatrix (nur fuer die zu messenden Groessen)
        self.R = R

        # dynamisch - gross Q, klein R
        # glaettung - klein Q, gross R
        return None

    def predict(self):
        self.x = np.dot(self.F, self.x)
        self.P = np.add(np.dot(np.dot(self.F, self.P), self.F.T), self.Q)
        return None
    
    def update(self, mesVec):
        # Innovation
        y = np.add(mesVec.T, - np.dot(self.H, self.x))

        # Residualkovarianz
        S = np.add(np.dot(np.dot(self.H, self.P), self.H.T), self.R)

        # Kalman - Gain
        K = np.dot(np.dot(self.P, self.H.T), np.linalg.pinv(S))
        
        # Zweite Zeile 0.0 setzen, damit sie den alten Abstand nicht verändert!!!
        # !!!
        K[1,0] = 0.0
        # !!!

        self.x = np.add(self.x, np.dot(K, y))
        self.P = np.add(self.P, - np.dot(np.dot(K,S),K.T))
        return None

    def resetCov(self):
        self.P = self.originP
        return None


    def getValue(self):
        return self.x