一些函数的定义
import sensor, image, time,utime
from pyb import RTC
class Vector:
def __init__(self, lis):
self._values = lis # 在vector中设置私有变量values存储数组数据
# 返回 dim 维零向量
@classmethod
def zero(cls, dim):
return cls([0] * dim)
# 返回向量的模
def norm(self):
return math.sqrt(sum(e ** 2 for e in self))
# 返回向量的单位向量
def normalize(self):
if self.norm() < 1e-8:
raise ZeroDivisionError("Normalize error! norm is zero.")
return 1 / self.norm() * Vector(self._values)
# 向量相加，返回结果向量
def __add__(self, other):
assert len(self) == len(other), "error in adding,length of vectors must be same"
return Vector([a + b for a, b in zip(self, other)])
# 向量相减，返回结果向量
def __sub__(self, other):
assert len(self) == len(other), "error in subbing,length of vectors must be same"
return Vector([a - b for a, b in zip(self, other)])
# 向量点乘，返回结果向量
def dot(self, other):
assert len(self) == len(other), \
"error in doting,length of vectors must be same."
return sum(a * b for a, b in zip(self, other))
# 向量左乘标量，返回结果向量
def __mul__(self, k):
return Vector([a * k for a in self])
# 向量右乘标量，返回结果向量
def __rmul__(self, k):
return Vector([a * k for a in self])
# 向量数量除法，返回结果向量
def __truediv__(self, k):
return 1 / k * self
# 向量取正
def __pos__(self):
return 1 * self
# 向量取负
def __neg__(self):
return -1 * self
# 取出向量的第index个元素,调用时直接vec[]
def __getitem__(self, index):
return self._values[index]
# 返回向量长度，调用时直接len(vec)
def __len__(self):
return len(self._values)
# 返回向量Vector(...)
def __repr__(self): # __repr__和__str__都在调用类时自动执行其中一个，倾向位置在最后一个
return "Vector({})".format(self._values)
# 返回向量(...),调用时直接print(vec)
def __str__(self):
return "({})".format(",".join(str(e) for e in self._values))
class Matrix:
def __init__(self, list2d):
self._values = [row[:] for row in list2d]
@classmethod
def zero(cls, r, c):
"""返回一个r行c列的零矩阵"""
return cls([[0] * c for _ in range(r)])
def T(self):
"""返回矩阵的转置矩阵"""
return Matrix([[e for e in self.col_vector(i)]
for i in range(self.col_num())])
def __add__(self, another):
"""返回两个矩阵的加法结果"""
assert self.shape() == another.shape(), \
"Error in adding. Shape of matrix must be same."
return Matrix([[a + b for a, b in zip(self.row_vector(i), another.row_vector(i))]
for i in range(self.row_num())])
def __sub__(self, another):
"""返回两个矩阵的减法结果"""
assert self.shape() == another.shape(), \
"Error in subtracting. Shape of matrix must be same."
return Matrix([[a - b for a, b in zip(self.row_vector(i), another.row_vector(i))]
for i in range(self.row_num())])
def dot(self, another):
"""返回矩阵乘法的结果"""
if isinstance(another, Vector):
# 矩阵和向量的乘法
assert self.col_num() == len(another), \
"Error in Matrix-Vector Multiplication."
return Vector([self.row_vector(i).dot(another) for i in range(self.row_num())])
if isinstance(another, Matrix):
# 矩阵和矩阵的乘法
assert self.col_num() == another.row_num(), \
"Error in Matrix-Matrix Multiplication."
return Matrix([[self.row_vector(i).dot(another.col_vector(j)) for j in range(another.col_num())]
for i in range(self.row_num())])
def __mul__(self, k):
"""返回矩阵的数量乘结果: self * k"""
return Matrix([[e * k for e in self.row_vector(i)]
for i in range(self.row_num())])
def __rmul__(self, k):
"""返回矩阵的数量乘结果: k * self"""
return self * k
def __truediv__(self, k):
"""返回数量除法的结果矩阵：self / k"""
return (1 / k) * self
def __pos__(self):
"""返回矩阵取正的结果"""
return 1 * self
def __neg__(self):
"""返回矩阵取负的结果"""
return -1 * self
def row_vector(self, index):
"""返回矩阵的第index个行向量"""
return Vector(self._values[index])
def col_vector(self, index):
"""返回矩阵的第index个列向量"""
return Vector([row[index] for row in self._values])
def __getitem__(self, pos):
"""返回矩阵pos位置的元素"""
r, c = pos
return self._values[r][c]
def ni_matrix(self):
"""返回矩阵的逆（二维矩阵）"""
a = self._values[0][0]
b = self._values[0][1]
c = self._values[1][0]
d = self._values[1][1]
ni1 = Matrix(([d, -b], [-c, a]))
k = 1 / (a * d - b * c)
return Matrix([[e * k for e in ni1.row_vector(i)]
for i in range(ni1.row_num())])
def size(self):
"""返回矩阵的元素个数"""
r, c = self.shape()
return r * c
def row_num(self):
"""返回矩阵的行数"""
return self.shape()[0]
__len__ = row_num
def col_num(self):
"""返回矩阵的列数"""
return self.shape()[1]
def shape(self):
"""返回矩阵的形状: (行数， 列数)"""
return len(self._values), len(self._values[0])
def __repr__(self):
return "Matrix({})".format(self._values)
__str__ = __repr__
初始化
# ________________________________Apriltags相关系数___________________________________________
f_x = (2.8 / 3.984) * 160 # find_apriltags defaults to this if not set
f_y = (2.8 / 2.952) * 120 # find_apriltags defaults to this if not set
c_x = 160 * 0.5 # find_apriltags defaults to this if not set (the image.w * 0.5)
c_y = 120 * 0.5 # find_apriltags defaults to this if not set (the image.h * 0.5)
K_x = 23
# ________________________________卡尔曼滤波器模型___________________________________________
##预测
# pre_Pk=(A.dot(Pk_1)).dot(A.T())+Q
# pre_Xkhat = A.dot(pre_Xk_1hat)
# pre_Xk_1hat = pre_Xkhat
##校正
# Xk_hat = pre_Xk + Kk.dot(Zk - pre_Xk)
# Kk=pre_Pk.dot((pre_Pk_1+R).ni_matrix())
# Pk = (I -Kk).dot(pre_Pk)
# ________________________________初始化噪声、误差协方差矩阵及相关变量___________________________________________
q1 = 1
q2 = 1
r1 = 1
r2 = 1
p1 = 1
p2 = 1
T = 0
A = Matrix([[1, T], [0, 1]])
H = Matrix([[1, 0], [0, 1]])
pre_xpk = Matrix([[0, 0], [0, 0]])
est_xpk_1 = Matrix([[0, 0], [0, 0]])
pre_xk = Matrix([[0], [0]])
est_xk = Matrix([[0], [0]])
est_xk_1 = Matrix([[0], [1]])
est_xpk = Matrix([[p1, 0], [0, p2]])
pre_ypk = Matrix([[0, 0], [0, 0]])
est_ypk_1 = Matrix([[0, 0], [0, 0]])
pre_yk = Matrix([[0], [0]])
est_yk = Matrix([[0], [0]])
est_yk_1 = Matrix([[0], [1]])
est_ypk = Matrix([[p1, 0], [0, p2]]) # 初始值
#K = 0
tag_cx_1 = 0
tag_cy_1 = 0
X_est = 0
XV_est = 0
Y_est = 0
YV_est = 0
MEA_XV = 0
MEA_cx = 0
MEA_YV = 0
MEA_cy = 0
rect2 = 0
rect3 = 0
#pre_V = 0
#pre_YV = 0
R = Matrix([[1, 0], [0, 1]]) # 测量噪声协方差矩阵
Q = Matrix([[5, 0], [0, 1]]) # 过程噪声协方差矩阵 第一个参数代表位置，第二个参数代表速度0
P = Matrix([[p1, 0], [0, p2]]) # 误差协方差矩阵
I = Matrix([[1, 0], [0, 1]]) # 单位矩阵
def kalman_x_filter(cx, vx):
global A, T, K, pre_xk, pre_xpk, est_xk_1, est_xpk_1, est_xpk, est_xk
# 预测阶段
pre_xk = A.dot(est_xk_1) # k时刻状态(x)预测值
pre_xpk = A.dot(est_xpk_1).dot(A.T()) + Q
# 更新阶段
# 卡尔曼增益
x_Zk = Matrix([[cx], [vx]]) # 观测矩阵
x_Kk = pre_xpk.dot(H.T()).dot((H.dot(pre_xpk).dot(H.T()) + R).ni_matrix()) # 观测矩阵H可视为1
# x_Kk = pre_xpk.dot((pre_xpk + R).ni_matrix())
# 状态更新
est_xk = pre_xk + x_Kk.dot(x_Zk - H.dot(pre_xk))
est_xk_1 = est_xk
# 协方差更新
est_xpk = (I - x_Kk).dot(pre_xpk)
est_xpk_1 = est_xpk
return est_xk.__getitem__([0, 0]), est_xk.__getitem__([1, 0])
def kalman_y_filter(cy, vy):
global A, T, K, pre_yk, pre_ypk, est_yk_1, est_ypk_1, est_ypk, est_yk
pre_yk = A.dot(est_yk_1)
pre_ypk = A.dot(est_ypk_1).dot(A.T()) + Q
y_Zk = Matrix([[cy], [vy]])
y_Kk = pre_ypk.dot(H.T()).dot((H.dot(pre_ypk).dot(H.T()) + R).ni_matrix())
# y_Kk = pre_ypk.dot((pre_ypk + R).ni_matrix())
est_yk = pre_yk + y_Kk.dot(y_Zk - H.dot(pre_yk))
est_yk_1 = est_yk
est_ypk = (I - y_Kk).dot(pre_ypk)
est_ypk_1 = est_ypk
return est_yk.__getitem__([0, 0]), est_yk.__getitem__([1, 0])主函数
#________________________________主函数___________________________________________
sensor.reset()
sensor.set_pixformat(sensor.RGB565)
sensor.set_framesize(sensor.QQVGA)
sensor.skip_frames(time = 2000)
sensor.set_auto_gain(False)
clock = time.clock()
rtc = RTC()
rtc.datetime((0, 0, 0, 0, 0, 0, 0, 0))
date1 = [0, 0, 0, 0, 0, 0, 0, 0]
while(True):
clock.tick()
img = sensor.snapshot()
find_tag = img.find_apriltags()
if find_tag:
date2 = rtc.datetime()
for tag in img.find_apriltags(fx=f_x, fy=f_y, cx=c_x, cy=c_y):
T = 1/255*(date1[7] - date2[7]) + date1[6] - date2[6] # 计算公式
date1[7] = date2[7]
date1[6] = date2[6]
MEA_XV = (tag.cx() - tag_cx_1) / T
MEA_VY = (tag.cy() - tag_cy_1) / T
MEA_cx=tag.cx()
MEA_cy=tag.cy()
tag_cx_1 = tag.cx()
tag_cy_1 = tag.cy()
A = Matrix([[1,T],[0,1]])
X_est, XV_est = kalman_x_filter(MEA_cx, MEA_XV)
Y_est, VY_est = kalman_y_filter(MEA_cy, MEA_VY)
# img.draw_rectangle(int(tag.cx()-(tag.rect()[2])/2),int(tag.cy()-(tag.rect()[3])/2),tag.rect()[2],tag.rect()[3],color = (255, 0, 0))
img.draw_cross(tag.cx(), tag.cy(), color = (255, 0, 0))
img.draw_rectangle(tag.rect(),color = (255, 0, 0))
img.draw_rectangle(int(X_est-(tag.rect()[2])/2),int(Y_est-(tag.rect()[3])/2),tag.rect()[2],tag.rect()[3],color = (0, 0, 255))
img.draw_cross(int(X_est), int(Y_est), color = (0, 0, 255))
rect2, rect3=tag.rect()[2],tag.rect()[3]
else: # 脱离目标采用卡尔曼滤波器进行预测
# A = Matrix([[1,T],[0,1]])
# a=1.6
# X_est, XV_est=kalman_x_filter(X_est,a*MEA_V)
# Y_est, YV_est=kalman_y_filter(Y_est,a*MEA_YV)
X_est, XV_est=kalman_x_filter(X_est,MEA_XV)
Y_est, YV_est=kalman_y_filter(Y_est,MEA_YV)
img.draw_rectangle(int(X_est-rect2/2),int(Y_est-rect3/2),rect2,rect3,color = (0, 0, 255))
img.draw_cross(int(X_est), int(Y_est), color = (0, 0, 255))
7万+
被折叠的 条评论
为什么被折叠?
到【灌水乐园】发言
