import sensor, image, time from pyb import RTC sensor.reset() sensor.set_pixformat(sensor.RGB565) sensor.set_framesize(sensor.QVGA) sensor.skip_frames(time = 2000) clock=time.clock() rtc=RTC() rtc.datetime((0,0,0,0,0,0,0,0)) #2*1列向量类 class Vector: def __init__(self,x,y): self.x=x self.y=y #运算符重载 def __add__(self,other): return Vector(self.x+other.x,self.y+other.y) def __sub__(self,other): return Vector(self.x-other.x,self.y-other.y) def __mul__(self，scalar): return Vector(self.x*scalar,self.y*scalar) def __rmul__(self,scalar): return self._mul_(scalar) def __str__(self): return f"Vector({self.x},{self.y})" #2*2矩阵类 class Matrix: def __init__(self,rows): self.rows=rows #二维列表格式[[a,b],[c,d]] def __mul__(self,other): if isinstance(other,Vector): #格式：矩阵*向量 return Vector(self.rows[0][0]*other.x+self.rows[0][1]*other.y, self.rows[1][0]*other.x+self.rows[1][1]*other.y) elif isinstance(other,Matrix): #格式：矩阵*矩阵 return Matrix([ [ self.rows[0][0]*other.rows[0][0]+self.rows[0][1]*other.rows[1][0], self.rows[0][0]*other.rows[0][1]+self.rows[0][1]*other.rows[1][1] ], [ self.rows[1][0]*other.rows[0][0]+self.rows[1][1]*other.rows[1][0], self.rows[1][0]*other.rows[0][1]+self.rows[1][1]*other.rows[1][1] ] ]) def inv(self): #仅限2*2非奇异矩阵 a,b=self.rows[0] c,d=self.rows[1] det=a*d-b*c return Matrix([ [d/det,-b/det], [-c/det,a/det] ]) def tran(self): return Matrix([[self.rows[0][0],self.rows[1][0]],[self.rows[0][1],self.rows[1][1]]]) def _str_(self): return f"Matrix({self.rows})" Q=Matrix([[5,0],[0,1]])#过程噪声协方差矩阵 第一个参数代表位置 第二个参数代表速度 R=Matrix([[1,0],[0,1]])#测量噪声协方差矩阵 I=Matrix([[1,0],[0,1]])#单位矩阵 class Kalman: def __Init__(self): self.dT=0.1 self.A=Matrix([[1,self.dT],[0,1]])#状态转移矩阵 self.H=Matrix([[1,0],[0,1]])#观测状态矩阵 self.pre=Vector(0,0)#先验估计 self.pre_P=Matrix([[0,0],[0,0]])#先验估计的协方差矩阵 self.est=Vector(0,0)#这一时刻的最优估计 self.est_P=Matrix([[X_P1,0],[0,X_P2]])#这一时刻最优估计的协方差矩阵 self.est_K=Matrix([[0,0],[0,0]])#这一时刻卡尔曼增益矩阵 self.last_time=rtc.datetime() def Kalman_filter(self,other): self.pre=self.A*self.est#更新这一时刻的先验估计，不考虑控制向量和控制矩阵的作用 self.pre_P=(self.A*self.est_P*self.A.tran())+Q#更新这一时刻先验估计的协方差矩阵 self.est_K=self.pre_P*((self.pre_P+R).inv())#更新卡尔曼增益，因观测矩阵为一单位矩阵，计算时可不考虑 #状态更新 self.est=self.pre+self.est_K*(self.Z-self.pre)#计算时省略了观测矩阵 #协方差更新 self.est_P=(I-self.est_K)*self.pre_P#计算时省略了观测矩阵 return(self.est.x,self.est.y) while(True): clock.tick() img=sensor.snapshot().gamma_corr(gamma = 0.5, contrast = 1.0, brightness = 0.0) img.gaussian(3) img.erode(1) blob_X=Kalman() blob_Y=Kalman() blobs_3 = img.find_blobs([(4, 70, 2, 52, -82, -10)],merge=True)#蓝色 for blob in blobs_3: if blob.roundness()>0.7 and blob.pixels()>100: current_time = rtc.datetime() dt = (current_time[6]*1e6 + current_time[7]) - (blob_X.last_time[6]*1e6 + blob_X.last_time[7]) dt = max(dt / 1e6, 0.01) # 转换为秒并限制最小值 #blob_X.est=Vector(blob[0],),blob_Y.est=blob[1] blob_X_est,blob_VX_est=Kalman_filter(blob_X) 报错AttributeError: 'Kalman' object has no attribute 'last_time'