python代码

import seaborn
import numpy as np 
import matplotlib.pyplot as plt


class 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'