|
|
大家好，我目前自己写了一个卡尔曼滤波算法，预测两个常数值，但是预测结果和跟踪结果很差。有没有大神看一下呢？
if __name__ == "__main__":
kalman = cv2.KalmanFilter(4, 2, 0)
while True:
state = 0.1 * np.random.randn(4, 2)
# 初始化
kalman.transitionMatrix = np.array([[1.,0., 1.,0.], [0., 1.,0., 1.],[0.,0., 1.,0.], [0., 0.,0., 1.]])
kalman.measurementMatrix = 1. * np.ones((2, 4))
kalman.processNoiseCov = 1e-5 * np.eye(4)
kalman.measurementNoiseCov = 1e-1 * np.ones((2, 2))
kalman.errorCovPost = 1. * np.ones((4, 4))
kalman.statePost = 0.1 * np.random.randn(4, 2)
while True:
state_angle = state[0]
# 预测
prediction = kalman.predict()
predict_X1 = prediction[0, 0]
predict_X2 = prediction[0, 1]
print("predict_X1", predict_X1)
print("predict_X2", predict_X2)
for i in range(100):
measurement = kalman.measurementNoiseCov * np.sin(i)
measurement=np.array([measurement,measurement+100],np.float32)
# 生成测量
measurement = np.dot(kalman.measurementMatrix, state) + measurement
measurement_X1 = measurement[0, 0,0]
measurement_X2 = measurement[0, 0,1]
print("measurement_X1",measurement_X1)
print("measurement_X2", measurement_X2)
# 校正预测与测量值差异
kalman.correct(measurement)
# 更新noise矩阵与状态
process_noise = sqrt(kalman.processNoiseCov[0,0]) * np.random.randn(4, 2)
state = np.dot(kalman.transitionMatrix, state) + process_noise
code = cv2.waitKey(100)
if code != -1:
break
if code in [27, ord('q'), ord('Q')]:
break
|
|
发表于 2019-1-23 17:45:45
发表于 2019-1-23 20:40:09
楼主