-
Notifications
You must be signed in to change notification settings - Fork 4
/
KalmanCV.py
93 lines (81 loc) · 3.73 KB
/
KalmanCV.py
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
import numpy as np
import cv2
import time
class KalmanFilter:
"""
KS:kalman模型
"""
def __init__(self):
self.oldTime=0
self.deltaTime=0
self.kf = cv2.KalmanFilter(6, 6)
self.kf.measurementMatrix = np.array(
[[1, 0, 0, 0, 0, 0], [0, 1, 0, 0, 0, 0], [0, 0, 1, 0, 0, 0], [0, 0, 0, 1, 0, 0],
[0, 0, 0, 0, 1, 0],
[0, 0, 0, 0, 0, 1]], np.float32)
self.kf.transitionMatrix = np.array(
[[1, 0, 0, 0, 0, 0], [0, 1, 0, 0, 0, 0], [0, 0, 1, 0, 0, 0], [0, 0, 0, 1, 0, 0],
[0, 0, 0, 0, 1, 0],
[0, 0, 0, 0, 0, 1]], np.float32)
self.kf.processNoiseCov = np.array(
[[1, 0, 0, 0, 0, 0], [0, 1, 0, 0, 0, 0], [0, 0, 1, 0, 0, 0], [0, 0, 0, 1, 0, 0],
[0, 0, 0, 0, 1, 0],
[0, 0, 0, 0, 0, 1]], np.float32)*8
# [[1, 0, 0, 0, 0.5, 0], [0, 1, 0, 0, 0, 0.5], [0, 0, 1, 0,0.5, 0], [0, 0, 0, 1, 0,0.5], [0.5, 0,0.5, 0, 3, 0],[0, 0.5, 0, 0.5, 0, 3]], np.float32) *8
self.kf.measurementNoiseCov = np.array(
[[1, 0, 0, 0, 0, 0], [0, 1, 0, 0, 0, 0], [0, 0, 1, 0, 0, 0], [0, 0, 0, 1, 0, 0],
[0, 0, 0, 0, 1, 0],
[0, 0, 0, 0, 0, 1]], np.float32) * 30
# [[1, 0, 0, 0, 0.5, 0], [0, 1, 0, 0, 0, 0.5], [0, 0, 1, 0, 0.5, 0], [0, 0, 0, 1, 0, 0.5],[0.5, 0, 0.5, 0, 3, 0],[0, 0.5, 0, 0.5, 0, 3]], np.float32) * 30
# self.kf = cv2.KalmanFilter(4, 2)
# self.kf.measurementMatrix = np.array([[1, 0, 0, 0], [0, 1, 0, 0]], np.float32)
# self.kf.transitionMatrix = np.array([[1, 0, 1, 0], [0, 1, 0,1], [0, 0, 1, 0], [0, 0, 0, 1]], np.float32)
# self.kf.processNoiseCov = np.array([[1, 0, 0, 0], [0, 1, 0, 0], [0, 0, 1, 0], [0, 0, 0, 1]], np.float32) * 0.5
# self.kf.measurementNoiseCov = np.array([[1, 0], [0, 1]], np.float32) * 0.3
# def Update(self, x, y=0):
# """
# KS:滤波
# """
# current_mes = np.array([[np.float32(x)], [np.float32(y)]])
# self.kf.correct(current_mes)
# current_pred = self.kf.predict()
# pred_x, pred_y = current_pred[0], current_pred[1]
# # print("kalmanPred: ", pred_x[0],pred_y[0])
#
# return int(pred_x[0]), int(pred_y[0])
def Correctx2(self, x1, y1, x2, y2, meanX, meanY):
"""
KS:滤波
"""
self.GetDeltaTime()
self.kf.transitionMatrix[0][4]=self.deltaTime
self.kf.transitionMatrix[1][5]=self.deltaTime
self.kf.transitionMatrix[2][4]=self.deltaTime
self.kf.transitionMatrix[3][5]=self.deltaTime
#
current_mes = np.array(
[[np.float32(x1)], [np.float32(y1)], [np.float32(x2)], [np.float32(y2)], [np.float32(meanX)],
[np.float32(meanY)]])
self.kf.correct(current_mes)
def Predictx2(self):
# """
# KS:预测
# """
self.GetDeltaTime()
self.kf.transitionMatrix[0][4] = self.deltaTime
self.kf.transitionMatrix[1][5] = self.deltaTime
self.kf.transitionMatrix[2][4] = self.deltaTime
self.kf.transitionMatrix[3][5] = self.deltaTime
current_pred = self.kf.predict()
pred_x1, pred_y1, pred_x2, pred_y2 = current_pred[0], current_pred[1], current_pred[2], current_pred[3]
# print("kalmanPred: ", pred_x[0],pred_y[0])
return int(pred_x1[0]), int(pred_y1[0]), int(pred_x2[0]), int(pred_y2[0])
# def Predict(self):
# current_pred = self.kf.predict()
# pred_x, pred_y = current_pred[0], current_pred[1]
# # print("kalmanPred: ", pred_x[0],pred_y[0])
# return int(pred_x[0]), int(pred_y[0])
def GetDeltaTime(self):
tempTime=time.time()
self.deltaTime=tempTime-self.oldTime
self.oldTime=tempTime