python代码

import seabornimport numpy as np import matplotlib.pyplot as pltclass Kalman:    def __init__(self):        self.F = np.mat([[1,0,.25,0], [0,1,0,.25], [0,0,1,0], [0,0,0,1]])        self.P = np.diag([.3, .3, .1, .1])        self.Q = np.diag([.3, .3, .3, .3])        self.H = np.diag([1, 1, 0, 0])        self.R = np.diag([2, 2, .1, .1])        self.file_out = file('out.txt', 'w')        self.EXCEPTION = 999                        def load_data(self, f):        dd = 0        for ll in open(f):            ss = ll.strip().split(' ')            if int(ss[3]) == 12:                x0 = float(ss[13])                y0 = float(ss[14])                dd += 1                if dd == 1: break        x = x0        y = y0        t = 0        vx = 0        vy = 0        dt = .25        X = np.mat([x,y,0,0]).T        exception_count = 0        r = []        v = []                ee = 0        for line in open(f):            s = line.strip().split(' ')            if int(s[3]) == 12:                ee += 1                        if ee == 1:                     continue                lx = x                ly = y                lt = t                x = float(s[13])                y = float(s[14])                t = float(s[25])                dx = x - lx                dy = y - ly                if np.abs(dx) > 5 or np.abs(dy) > 5 or (x == 0 and y == 0):                    exception_count += 1                    continue                elif np.abs(dx) < 5 and np.abs(dy) < 5:                    record_exception = exception_count                    exception_count = 0                    if record_exception > self.EXCEPTION:       # EXCEPTION                        X = np.mat([x,y,0,0]).T                    else:                        lX = X                        X_ = self.F * lX                        P_ = self.F * self.P * self.F.T + self.Q                        K = P_ * self.H.T * (self.H * P_ * self.H.T + self.R).I                        Z = np.mat([x, y, 0, 0]).T                        X = X_ + K * (Z - self.H * X_)                        P = (np.eye(4) - K * self.H) * P_                r.append([float(X[0]), float(X[1])])                v.append([float(X[2]), float(X[3])])                self._write_data_to_a_file(X[0], X[1])        xy = np.array(r)        plt.plot(xy[:,0], xy[:,1], '.')        plt.show()    def _write_data_to_a_file(self, x, y):        self.file_out.write('%f %f\n' % (x, y))if __name__ == '__main__':    import time    start_time = time.time()    k = Kalman()    k.load_data(f)    end_time = time.time()    time_use = end_time - start_time    print 'Time consumed : ', time_use * 1000, 'ms'