jens
8f9f19e0c3
git-svn-id: http://moon:8086/svn/projects/HendiControl@210 fda53097-d464-4ada-af97-ba876c37ca34
142 lines
2.7 KiB
Python
142 lines
2.7 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']
|
|
var_Z = params['var_Z']
|
|
model = np.matrix([1, dt, 1/2*dt**2]).transpose()
|
|
N = len(model)-1
|
|
|
|
P = var_P*np.eye(N)
|
|
R = var_R*np.eye(N)
|
|
H = np.eye(N)
|
|
|
|
A = np.eye(N)
|
|
for row in range(0, N):
|
|
A[row, row:N] = model.transpose()[0, 0:N-row]
|
|
|
|
G = np.matrix(model[N:0:-1])
|
|
Q = G * G.transpose() * var_Q
|
|
|
|
self.P = P
|
|
self.Q = Q
|
|
self.R = R
|
|
self.N = N
|
|
self.A = A
|
|
self.H = H
|
|
self.var_Z = var_Z
|
|
|
|
X = np.matrix([0, 1.0]).transpose()
|
|
Xp = np.matrix([0, 0]).transpose()
|
|
self.Xp = Xp
|
|
self.X = X
|
|
|
|
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):
|
|
|
|
Y = np.matrix([y[0], y[1]]).transpose()
|
|
# ----------------------------
|
|
# Take noisy measurement
|
|
Z = self.H * Y + self.var_Z * np.random.randn(self.N, 1)
|
|
|
|
return Z
|
|
|
|
def process(self, Z):
|
|
|
|
# ----------------------------
|
|
# State estimate
|
|
self.Xp = self.A * self.Xp
|
|
|
|
# ----------------------------
|
|
# Measurement prediction
|
|
Zp = self.H * self.Xp
|
|
|
|
# ----------------------------
|
|
# Measurement residual
|
|
V = Z - Zp
|
|
|
|
# ----------------------------
|
|
# 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)
|
|
|
|
# ----------------------------
|
|
# 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__':
|
|
|
|
params = {
|
|
'dt' : 1,
|
|
'var_P' : 1,
|
|
'var_Q' : 0,
|
|
'var_R' : 1,
|
|
'var_Z' : 0
|
|
}
|
|
k = Kalman(params)
|
|
|
|
_x1 = np.empty(0)
|
|
_y1 = np.empty(0)
|
|
_x2 = np.empty(0)
|
|
_y2 = np.empty(0)
|
|
|
|
seqn = range(0, 100)
|
|
|
|
for n in seqn:
|
|
X = k.process_truth()
|
|
Z = k.process_measurement((X[0,0], 0))
|
|
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")
|
|
|