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()
