def kalman_initial(q): kf = KalmanFilter(transition_matrices=np.mat([[1, 0.1], [0, 1]]), observation_matrices=np.mat([1, 0])) ''' kf.transition_matrices = np.mat([[1, 0, 0, 1, 0, 0], [0, 1, 0, 0, 1, 0], [0, 0, 1, 0, 0, 1], [0, 0, 0, 1, 0, 0], [0, 0, 0, 0, 1, 0], [0, 0, 0, 0, 0, 1]]) kf.observation_matrices = np.mat([[1, 0, 0, 0, 0, 0], [0, 1, 0, 0, 0, 0], [0, 0, 1, 0, 0, 0]]) kf.observation_covariance = np.eye(3) # kf0.initial_state_covariance = np.eye(6) kf.transition_covariance = np.eye(6) kf.n_dim_obs = 3 kf.n_dim_state = 6 ''' # 定义初始状态协方差矩阵 kf.initial_state_covariance = np.mat([[1, 0], [0, 1]]) # 定义状态转移矩阵,因为每秒钟采一次样,所以delta_t = 1 kf.transition_matrices = np.mat([[1, 1], [0, 1]]) # 定义状态转移协方差矩阵,这里我们把协方差设置的很小,因为觉得状态转移矩阵准确度高 kf.transition_covariance = np.mat([[q, 0], [0, q]]) # 定义观测矩阵 kf.observation_matrices = np.mat([1, 0]) # 定义观测噪声协方差 kf.observation_covariance = np.mat([758]) return kf
def demo(): img = np.zeros((768, 1024, 3), np.uint8) trajs = deque(maxlen=200) trajs_filterd = deque(maxlen=200) def draw(event, x, y, flags, param): kf, fm, fo = param if event == cv2.EVENT_MOUSEMOVE: fm[:], fo[:] = kf.filter_update(fm, fo, np.array([x, y])) fx = int(fm[0]) fy = int(fm[1]) trajs.append((x, y)) trajs_filterd.append((fx, fy)) cv2.namedWindow("image") kf = KalmanFilter(n_dim_state=4, n_dim_obs=2) kf.transition_matrices = np.array([[1, 0, 1, 0], [0, 1, 0, 1], [0, 0, 1, 0], [0, 0, 0, 1]]) kf.observation_matrices = np.array([[1, 0, 0, 0], [0, 1, 0, 0]]) kf.transition_covariance = 1e-3 * np.eye(4) kf.observation_covariance = 10 * np.eye(2) kf.initial_state_mean = np.array([0, 0, 0, 0]) kf.initial_state_covariance = np.eye(4) fm = kf.initial_state_mean fo = kf.initial_state_covariance cv2.setMouseCallback("image", draw, (kf, fm, fo)) while True: img = np.zeros((768, 1024, 3), np.uint8) for i in range(len(trajs) - 1): cv2.line(img, trajs[i], trajs[i + 1], (0, 255, 0), 1) cv2.line(img, trajs_filterd[i], trajs_filterd[i + 1], (0, 0, 255), 1) map(lambda pt: cv2.circle(img, pt, 3, (0, 255, 0), 1), trajs) map(lambda pt: cv2.circle(img, pt, 3, (0, 0, 255), 1), trajs_filterd) cv2.imshow("image", img) if cv2.waitKey(5) & 0xFF == ord("q"): return
def post(self): jsonData = request.get_json(force=True) # strRawData = jsonData["raw_data"] strRawData = '[{"datetime":"2019-02-20 17:24:09","latitude":26.4674398,"longitude":87.2838534,"altitude":100},{"datetime":"2019-02-20 17:24:24","latitude":26.4674554,"longitude":87.2837988,"altitude":100},{"datetime":"2019-02-20 17:24:33","latitude":26.4674697,"longitude":87.2837883,"altitude":100},{"datetime":"2019-02-20 17:24:48","latitude":26.4674697,"longitude":87.5557883,"altitude":100}]' data = json.loads(strRawData) logging.info(data) observed_lat = [] observed_lon = [] observed_alt = [] obs_dt = [] for i in range(len(data)): observed_lat = np.append(observed_lat, data[i]['latitude']) observed_lon = np.append(observed_lon, data[i]['longitude']) observed_alt = np.append(observed_alt, data[i]['altitude']) obs_dt = np.append(obs_dt, data[i]['datetime']) coords = pd.DataFrame({ 'lat': observed_lat, 'lon': observed_lon, 'ele': observed_alt, 'time': obs_dt }) logging.info(coords) measurements = np.ma.masked_invalid(coords[['lon', 'lat', 'ele']].values) logging.info(measurements) # transition_matrices F = np.array([[1, 0, 0, 1, 0, 0], [0, 1, 0, 0, 1, 0], [0, 0, 1, 0, 0, 1], [0, 0, 0, 1, 0, 0], [0, 0, 0, 0, 1, 0], [0, 0, 0, 0, 0, 1]]) # observation_matrices H = np.array([[1, 0, 0, 0, 0, 0], [0, 1, 0, 0, 0, 0], [0, 0, 1, 0, 0, 0]]) # observation_covariance R = np.diag([1e-4, 1e-4, 100])**2 initial_state_mean = np.hstack([measurements[0, :], 3 * [0.]]) initial_state_covariance = np.diag([1e-4, 1e-4, 50, 1e-6, 1e-6, 1e-6])**2 kf = KalmanFilter(transition_matrices=F, observation_matrices=H, observation_covariance=R, initial_state_mean=initial_state_mean, initial_state_covariance=initial_state_covariance, em_vars=['transition_covariance']) Q = np.array([[ 3.17720723e-09, -1.56389148e-09, -2.41793770e-07, 2.29258935e-09, -3.17260647e-09, -2.89201471e-07 ], [ 1.56687815e-09, 3.16555076e-09, 1.19734906e-07, 3.17314157e-09, 2.27469595e-09, -2.11189940e-08 ], [ -5.13624053e-08, 2.60171362e-07, 4.62632068e-01, 1.00082746e-07, 2.81568920e-07, 6.99461902e-05 ], [ 2.98805710e-09, -8.62315114e-10, -1.90678253e-07, 5.58468140e-09, -5.46272629e-09, -5.75557899e-07 ], [ 8.66285671e-10, 2.97046913e-09, 1.54584155e-07, 5.46269262e-09, 5.55161528e-09, 5.67122163e-08 ], [ -9.24540217e-08, 2.09822077e-07, 7.65126136e-05, 4.58344911e-08, 5.74790902e-07, 3.89895992e-04 ]]) Q = 0.5 * (Q + Q.T) kf.transition_covariance = Q state_means, state_vars = kf.smooth(measurements) newData = data mX = [] mY = [] mA = [] mB = [] for i in range(len(data)): tempLatitude = state_means[i][1] tempLongitude = state_means[i][0] newData[i]['latitude'] = tempLatitude newData[i]['longitude'] = tempLongitude mX = np.append(mX, tempLatitude) mY = np.append(mY, tempLongitude) mA = np.append(mA, newData[i]["altitude"]) mB = np.append(mB, newData[i]["datetime"]) newCords = pd.DataFrame({'lat': mX, 'lon': mY, 'ele': mA, 'time': mB}) logging.info(newCords) finalResult = json.dumps(newData) logging.info(finalResult) return {'result': finalResult}, 200
import random N = 3 * 500 sigma = 0.7 # Function to estimate W = numpy.zeros(N) W[0 : 500 ] = 1 W[500 : 1000 ] = 3 W[1000 : 1500 ] = 2 # Generate noisy measures X = numpy.zeros(N) for i in range(N): X[i] = W[i] + random.gauss(0, sigma) kf = KalmanFilter(initial_state_mean=X[0], n_dim_obs=1) kf.transition_covariance = 1e-3 Z = kf.em(X).smooth(X)[0] #Z = kf.smooth(X)[0] plt.figure(1) plt.plot( X, 'r:' ) plt.plot( W, 'b-' ) plt.plot( Z, 'g-' ) plt.show()