an.tsouchlos/kalman_filter
1
0
Fork 0
Code Issues Pull Requests Projects Releases Wiki Activity

Implemented second order LTI system example

Browse Source
This commit is contained in:
Andreas Tsouchlos 2022-04-08 02:46:55 +02:00
parent d83bbda21e
commit daa1a355ea
2 changed files with 53 additions and 19 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

68
kalman_filter/__main__.py
View File

@ -1,29 +1,65 @@
from .kalman_filter import *
import matplotlib.pyplot as plt
def print_state(x, P, K):
def print_state(x, P):
print("------------")
print("x: " + str(x))
print("P: " + str(P))
print("K: " + str(K))
"""
x_{n+1} = x_n + dt*v_n
v_{n+1} = v_n
"""
def main():
# 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)
n_iterations = 500
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)
x_actual = np.zeros([n_iterations, 2])
x_kalman = np.zeros([n_iterations, 2])
x_measurement = np.zeros([n_iterations, 2])
std_dist = [30, 0.5]
x0_actual = [50, 3]
x0 = [100, 50]
P0 = [[10, 0], [0, 5]]
H = [[1, 0], [0, 1]]
Q = [[0.2, 0], [0, 2]]
F = [[1, 1], [0, 1]]
G = 0
R = [[std_dist[0]**2, 0], [0, std_dist[1]**2]]
# R = [[std_dist[0], 0], [0, std_dist[1]]]
f = KalmanFilter(x0, P0, H, Q, F, G)
x_actual[0] = x0_actual
x_kalman[0] = x0
for i in range(1, n_iterations):
measurement = dot(F,x_actual[i-1]) + np.random.normal([0,0], std_dist)
f.predict(0)
f.update(measurement, R)
print_state(f.x, f.P)
x_actual[i] = dot(F, x_actual[i-1])
x_kalman[i] = f.x
x_measurement[i] = measurement
t = np.linspace(0, n_iterations, n_iterations)
plt.plot(t, x_actual[:, 0])
plt.plot(t, x_kalman[:, 0])
plt.plot(t, x_measurement[:, 0])
plt.show()
if __name__ == '__main__':
main()

4
kalman_filter/kalman_filter.py
View File

@ -16,7 +16,6 @@ class KalmanFilter:
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
@ -24,8 +23,7 @@ class KalmanFilter:
inv = inv_gen(temp)
self.K = dot(self.P, dot(np.transpose(self.H), inv))
K = self.K
K = dot(self.P, dot(np.transpose(self.H), inv))
innovation = z - dot(self.H, self.x)
self.x = self.x + dot(K, innovation)