157 lines
3.1 KiB
Python
157 lines
3.1 KiB
Python
import numpy as np
|
|
from numpy.linalg import inv
|
|
from matplotlib.pyplot import plot, figure, subplot, title, xlabel, ylabel, grid, show
|
|
|
|
|
|
class Kalman:
|
|
def __init__(self, params={}):
|
|
dt = params['dt']
|
|
var_P = params['var_P']
|
|
var_Q = params['var_Q']
|
|
var_R = params['var_R']
|
|
model = np.matrix([1, dt, 1/2*dt**2]).transpose()
|
|
N = len(model)-1
|
|
|
|
# Process Covariance Matrix
|
|
self.P = var_P*np.eye(N)
|
|
|
|
# Sensor Noise Covariance Matrix
|
|
self.R = var_R*np.eye(N)
|
|
H = np.eye(N)
|
|
|
|
self.A = np.eye(N)
|
|
for row in range(0, N):
|
|
self.A[row, row:N] = model.transpose()[0, 0:N-row]
|
|
|
|
G = np.matrix(model[N:0:-1])
|
|
|
|
# Process Noise Covariance Matrix
|
|
self.Q = G * G.transpose() * var_Q
|
|
|
|
self.N = N
|
|
self.H = H
|
|
|
|
# State Matrix
|
|
self.X = np.matrix([0, 1.0]).transpose()
|
|
|
|
# Predicted State Matrix
|
|
self.Xp = np.matrix([0, 0]).transpose()
|
|
|
|
np.set_printoptions(precision=3)
|
|
|
|
@staticmethod
|
|
def print(p, d):
|
|
print(p)
|
|
print(d)
|
|
|
|
def initial(self, X):
|
|
self.Xp = np.matrix([X[0], X[1]]).transpose()
|
|
|
|
def process_truth(self):
|
|
# ----------------------------
|
|
# Process ground truth
|
|
self.X = self.A * self.X
|
|
|
|
return self.X
|
|
|
|
def process_measurement(self, y, var_Z):
|
|
|
|
Y = np.matrix([y[0], y[1]]).transpose()
|
|
# ----------------------------
|
|
# Take noisy measurement
|
|
Z = self.H * Y + var_Z * np.random.randn(self.N, 1)
|
|
|
|
return Z
|
|
|
|
def process(self, Z):
|
|
|
|
# ----------------------------
|
|
# State estimate
|
|
self.Xp = self.A * self.Xp
|
|
|
|
# ----------------------------
|
|
# State prediction covariance
|
|
self.P = self.A * self.P * self.A.transpose() + self.Q
|
|
|
|
# ----------------------------
|
|
# Measurement prediction covariance
|
|
S = self.H * self.P * self.H.transpose() + self.R
|
|
|
|
# ----------------------------
|
|
# Kalman gain
|
|
K = self.P * self.H.transpose() * inv(S)
|
|
print("Kalman gain = {}".format(K))
|
|
|
|
# ----------------------------
|
|
# Measurement prediction
|
|
Zp = self.H * self.Xp
|
|
|
|
# ----------------------------
|
|
# Measurement residual
|
|
V = Z - Zp
|
|
|
|
# Update state estimate
|
|
self.Xp = self.Xp + K * V
|
|
|
|
# ----------------------------
|
|
# Updated state covariance
|
|
self.P = self.P - K * S * K
|
|
|
|
return self.Xp
|
|
|
|
|
|
# Main
|
|
if __name__ == '__main__':
|
|
dt = 1
|
|
|
|
params = {
|
|
'dt' : dt,
|
|
'var_P' : 1,
|
|
'var_Q' : 0.0001,
|
|
'var_R' : 1
|
|
}
|
|
k = Kalman(params)
|
|
|
|
_x1 = np.empty(0)
|
|
_y1 = np.empty(0)
|
|
_x2 = np.empty(0)
|
|
_y2 = np.empty(0)
|
|
|
|
N = int(1000/dt)
|
|
seqn = range(0, N)
|
|
|
|
_seqn = range(0, 2*N)
|
|
for n in seqn:
|
|
X = (1, 0)
|
|
Z = k.process_measurement(X, 0.1)
|
|
# Kalman.print("Z:", Z)
|
|
Xp = k.process(Z)
|
|
|
|
_x1 = np.append(_x1, Z[0])
|
|
_x2 = np.append(_x2, Z[1])
|
|
_y1 = np.append(_y1, Xp[0])
|
|
_y2 = np.append(_y2, Xp[1])
|
|
|
|
for n in seqn:
|
|
X = k.process_truth()
|
|
Z = k.process_measurement((X[0,0], 0), 0.1)
|
|
# Kalman.print("Z:", Z)
|
|
Xp = k.process(Z)
|
|
|
|
_x1 = np.append(_x1, Z[0])
|
|
_x2 = np.append(_x2, Z[1])
|
|
_y1 = np.append(_y1, Xp[0])
|
|
_y2 = np.append(_y2, Xp[1])
|
|
|
|
figure(1)
|
|
subplot(2, 1, 1)
|
|
plot(_seqn, _x1, 'bx', _seqn, _y1, '-r', linewidth=1)
|
|
grid(True)
|
|
subplot(2, 1, 2)
|
|
plot(_seqn, _x2, 'bx', _seqn, _y2, '-r', linewidth=1)
|
|
grid(True)
|
|
show()
|
|
|
|
print("End of program")
|
|
|