kalman-filter python实现？

本文主要是介绍kalman-filter python实现？，希望对大家解决编程问题提供一定的参考价值，需要的开发者们随着小编来一起学习吧！

上代码

import numpy as npclass KalmanFilter(object):def __init__(self, F = None, B = None, H = None, Q = None, R = None, P = None, x0 = None):if(F is None or H is None):raise ValueError("Set proper system dynamics.")self.n = F.shape[1]self.m = H.shape[1]self.F = Fself.H = Hself.B = 0 if B is None else Bself.Q = np.eye(self.n) if Q is None else Qself.R = np.eye(self.n) if R is None else Rself.P = np.eye(self.n) if P is None else Pself.x = np.zeros((self.n, 1)) if x0 is None else x0def predict(self, u = 0):self.x = np.dot(self.F, self.x) + np.dot(self.B, u)self.P = np.dot(np.dot(self.F, self.P), self.F.T) + self.Qreturn self.xdef update(self, z):y = z - np.dot(self.H, self.x)S = self.R + np.dot(self.H, np.dot(self.P, self.H.T))K = np.dot(np.dot(self.P, self.H.T), np.linalg.inv(S))self.x = self.x + np.dot(K, y)I = np.eye(self.n)self.P = np.dot(np.dot(I - np.dot(K, self.H), self.P), (I - np.dot(K, self.H)).T) + np.dot(np.dot(K, self.R), K.T)def example():dt = 1.0/60F = np.array([[1, dt, 0], [0, 1, dt], [0, 0, 1]])H = np.array([1, 0, 0]).reshape(1, 3)Q = np.array([[0.05, 0.05, 0.0], [0.05, 0.05, 0.0], [0.0, 0.0, 0.0]])R = np.array([0.5]).reshape(1, 1)x = np.linspace(-10, 10, 100)measurements = - (x**2 + 2*x - 2)  + np.random.normal(0, 2, 100)kf = KalmanFilter(F = F, H = H, Q = Q, R = R)predictions = []for z in measurements:predictions.append(np.dot(H,  kf.predict())[0])kf.update(z)import matplotlib.pyplot as pltplt.plot(range(len(measurements)), measurements, label = 'Measurements')plt.plot(range(len(predictions)), np.array(predictions), label = 'Kalman Filter Prediction')plt.legend()plt.show()if __name__ == '__main__':example()