openmv卡尔曼滤波

# Kalman_Apriltags - By: 就要吃两碗饭 - 周一 7月 25 2022 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 R = Matrix([[r1,0],[0,r2]])#测量噪声协方差矩阵 Q = Matrix([[r1,0],[0,r2]])#过程噪声协方差矩阵 P = Matrix([[p1,0],[0,p2]])#误差协方差矩阵 I = Matrix([[1,0],[0,1]])#单位矩阵 T = 0 A = Matrix([[1,T],[0,1]]) H= Matrix([[1,0],[0,1]]) pre_Pk =Matrix([[0,0],[0,0]]) pre_Pk_1=Matrix([[0,0],[0,0]]) pre_Xkhat = Matrix([[0],[0]]) Xkhat = Matrix([[0],[0]]) Xk_1hat=Matrix([[0],[1]]) Pk_1=Matrix([[p1,0],[0,p2]]) pre_Xk_1hat=Matrix([[0,0],[0,0]]) pre_YPk =Matrix([[0,0],[0,0]]) pre_YPk_1=Matrix([[0,0],[0,0]]) pre_Ykhat = Matrix([[0],[0]]) Ykhat = Matrix([[0],[0]]) Yk_1hat=Matrix([[0],[1]]) YPk_1=Matrix([[p1,0],[0,p2]])#初�