基于openmv的卡尔曼滤波（简化版）

一些函数的定义 

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列的零矩阵""