From d83bbda21e6e52bc419f7621c733cb625a01a555 Mon Sep 17 00:00:00 2001
From: Andreas Tsouchlos <an.tsouchlos@gmail.com>
Date: Fri, 8 Apr 2022 01:11:43 +0200
Subject: [PATCH] First draft of filter implementation

---
 .gitignore                                    |   1 +
 kalman_filter/__main__.py                     |  26 +++++++++-
 .../__pycache__/__init__.cpython-310.pyc      | Bin 150 -> 0 bytes
 .../__pycache__/__main__.cpython-310.pyc      | Bin 342 -> 0 bytes
 .../__pycache__/filter.cpython-310.pyc        | Bin 148 -> 0 bytes
 kalman_filter/filter.py                       |   0
 kalman_filter/kalman_filter.py                |  49 ++++++++++++++++++
 7 files changed, 74 insertions(+), 2 deletions(-)
 create mode 100644 .gitignore
 delete mode 100644 kalman_filter/__pycache__/__init__.cpython-310.pyc
 delete mode 100644 kalman_filter/__pycache__/__main__.cpython-310.pyc
 delete mode 100644 kalman_filter/__pycache__/filter.cpython-310.pyc
 delete mode 100644 kalman_filter/filter.py
 create mode 100644 kalman_filter/kalman_filter.py

diff --git a/.gitignore b/.gitignore
new file mode 100644
index 0000000..709c80b
--- /dev/null
+++ b/.gitignore
@@ -0,0 +1 @@
+kalman_filter/__pycache__
diff --git a/kalman_filter/__main__.py b/kalman_filter/__main__.py
index 2f97058..d88db32 100644
--- a/kalman_filter/__main__.py
+++ b/kalman_filter/__main__.py
@@ -1,7 +1,29 @@
-from .filter import *
+from .kalman_filter import *
+
+
+def print_state(x, P, K):
+    print("------------")
+    print("x: " + str(x))
+    print("P: " + str(P))
+    print("K: " + str(K))
+
 
 def main():
-    print("Hello World!")
+#    f = KalmanFilter(x0=[100, 20], P0=[[50, 0],[0, 10]], H=np.identity(2), Q=[[0, 0], [0,0]], F=[[1, 1], [0,1]], G=np.zeros([2, 2]))
+    f = KalmanFilter(x0=10, P0=5, H=1, Q=0, F=1, G=0)
+
+    print_state(f.x, f.P, f.K)
+    f.predict(0)
+    print_state(f.x, f.P, f.K)
+    f.predict(0)
+    print_state(f.x, f.P, f.K)
+    f.predict(0)
+    f.update(z=6, R=5)
+    print_state(f.x, f.P, f.K)
+    f.predict(0)
+    print_state(f.x, f.P, f.K)
+    f.predict(0)
+    print_state(f.x, f.P, f.K)
 
 if __name__ == '__main__':
     main()
diff --git a/kalman_filter/__pycache__/__init__.cpython-310.pyc b/kalman_filter/__pycache__/__init__.cpython-310.pyc
deleted file mode 100644
index e0b8410084494dc81fbbd4d01c7b35c1003c0949..0000000000000000000000000000000000000000
GIT binary patch
literal 0
HcmV?d00001

literal 150
zcmd1j<>g`k0#;4`BoO@=L?8o3AjbiSi&=m~3PUi1CZpd<h9ZzKg7{^rpOK%Ns-Kvb
zQk0rlte>7)qMx0Zlbe_qpO%?Zl3Ikui;vID%PfhH*DI*J#bJ}1pHiBWY6mj1m<dR*
GFaQ9l;vuX6

diff --git a/kalman_filter/__pycache__/__main__.cpython-310.pyc b/kalman_filter/__pycache__/__main__.cpython-310.pyc
deleted file mode 100644
index ffe769f179273844d3eaf4e3bb8354dab7e1335f..0000000000000000000000000000000000000000
GIT binary patch
literal 0
HcmV?d00001

literal 342
zcmZ8dJxc^J5S__>upE1Z|3Did7H%zw2sTz{ae`nIqLVw9%g2#j3vJIo<6qj=%GT0G
zCwurg@ZQWL^JbES>2v}dZ}Q@XIKL$EI|PeUMz_yIKxp9-NKp0!*JuRgD-m}LZUBLb
z`3PCQA~#DM@Zgq<Kau90F}j+m=Xo1sC(J>L*<*cftPQ(YAzIpFf3c3O@1G&&+&t3z
zQF9lZX|$(kbk{7~zFBGOv{#GP_9p&QN;%znr4H8*L!Qbk?M|1dF_xT$>Z@fYv!UD$
j`9AU58Kr*KZ>wVT9lJpH)|{lbcIgu<s6>@bF#q@j)&NPP

diff --git a/kalman_filter/__pycache__/filter.cpython-310.pyc b/kalman_filter/__pycache__/filter.cpython-310.pyc
deleted file mode 100644
index 8cdd6318d8b2eceb878a39e641b46db16d5d38d8..0000000000000000000000000000000000000000
GIT binary patch
literal 0
HcmV?d00001

literal 148
zcmd1j<>g`k0x3=ZBoO@=L?8o3AjbiSi&=m~3PUi1CZpd<h9ZzKg7{^wpOK%Ns-Kvb
zQk0rlte>7)qMx0Zlbe_qpO%?Zl3Ikug9z#sRNmsS$<0qG%}KQb8Cc8&Bv=>#Z80Gg

diff --git a/kalman_filter/filter.py b/kalman_filter/filter.py
deleted file mode 100644
index e69de29..0000000
diff --git a/kalman_filter/kalman_filter.py b/kalman_filter/kalman_filter.py
new file mode 100644
index 0000000..0be6a8c
--- /dev/null
+++ b/kalman_filter/kalman_filter.py
@@ -0,0 +1,49 @@
+import numpy as np
+from numpy import dot
+
+
+def inv_gen(A):
+    if np.isscalar(A):
+        return 1 / A
+    else:
+        return np.linalg.inv(A)
+
+class KalmanFilter:
+    def __init__(self, x0, P0, H, Q, F, G):
+        self.x = x0
+        self.P = P0
+        self.H = H
+        self.Q = Q
+        self.F = F
+        self.G = G
+        self.K = 1
+
+    def update(self, z, R):
+        temp = dot(self.H, dot(self.P,self.H)) + R
+
+
+        inv = inv_gen(temp)
+
+        self.K = dot(self.P, dot(np.transpose(self.H), inv))
+        K = self.K
+
+        innovation = z - dot(self.H, self.x)
+        self.x = self.x + dot(K, innovation)
+
+        KH = dot(K, self.H)
+        
+
+        K_complementary = 0
+
+        if np.isscalar(temp):
+            K_complementary = 1 - KH
+        else:
+            K_complementary = np.identity(np.shape(KH)[0]) - KH
+
+
+        self.P = dot(K_complementary, dot(self.P, np.transpose(K_complementary))) + dot(K, dot(R, np.transpose(K)))
+
+    def predict(self, u):
+        self.x = dot(self.F, self.x) + dot(self.G,u)
+        self.P = dot(self.F, dot(self.P, inv_gen(self.F))) + self.Q
+