卡尔曼滤波测量球体自由落下

柔光的暖阳◎ 2022-10-05 12:49 81阅读 0赞

#                              卡尔曼滤波测量球体自由落下 #

![2021060907043725.png][]

![watermark_type_ZmFuZ3poZW5naGVpdGk_shadow_10_text_aHR0cHM6Ly9ibG9nLmNzZG4ubmV0L3FxXzE1MjA0MTc5_size_16_color_FFFFFF_t_70][]

"""
    Created by Kaijun on 2020/11/11
    """
    import cv2 as cv
    # import cv2
    import numpy as np
    from scipy.ndimage.filters import gaussian_filter
    from filterpy.kalman import predict,update
    from filterpy.stats import  gaussian
    from filterpy.common import Q_discrete_white_noise
    import time
    
    import numpy as np
    import numpy.linalg as la
    
    
    '''
    三个步骤：
    1.初始化：初始状态
    2.预测： x=Fx+Bu Q p
    3.更新： z：1维的测量值， H：矩阵（变换矩阵，维度） R：噪声
    init_x：初始化状态 ， P F B u Q  
    z H R
    #------------------------------------------------
    # z: 测量值 （把这里测量球的圆心传进去）
    # x：跟踪物体机器人的状态，
    # P: 预测的方差,这个值会在卡尔曼滤波的过程中更新
    # Q: 在预测时,系统产生的过程噪声. 例如我们并不准确知道小车当前速度
    # R: 测量噪声,通常情况下,测量设备不变,那么测量噪声的值也不会发生变化
    # K: 卡尔曼增益,这个很重要,它是我们在测量值和预测值之间进行取舍的缩放因子
    # F：状态转移矩阵
    # B，u：控制方程外部输入的控制 不是必须的，比如外部加速度
    B = 0 可以为0
    u = 0 可以为0
    '''
    #开始卡尔曼滤波函数
    #卡尔曼滤波 初始化 预测 更新
    def kalman(mu,P,F,Q,B,u,z,H,R):
        # mu, P : 预测值, 协方差矩阵
        # F, Q : 状态转义矩阵 , 过程噪声
        # B, u : 控制向量
        # z : 测量值
        # H, R : 测量矩阵,测量方差
        # 预测的步骤
        mup = F @ mu + B @ u; #预测过程
        pp = F @ P @ F.T + Q; #更新预测协方差矩阵
    
    
    
        # 若没有传入测量的结果,则直接将预测的值返回 【这里指圆心的值】 【不传z值的话只走预测不走更新】
        if z is None:
            return mup, pp
    
        # 更新的步骤  z:2*1  -  H:2*4 @ mu:4*1
        y = z - H @ mup # 用测量值 - 预测值
    
        k = pp @ H.T @ la.inv(H @ pp @ H.T +R) # 计算卡尔曼增益
    
        new_mu = mup + k @ y; # 计算卡尔曼融合结果
        new_P = (np.eye(len(P))-k @ H) @ pp; # 计算融合之后的协方差矩阵
        return new_mu, new_P
    
    
    #测量球的位置
    def rgbh(xs,mask):
        def normhist(x): return x / np.sum(x)
        def h(rgb):
            return cv.calcHist([rgb]
                                ,[0, 1, 2]
                                ,imCropMask
                                ,[256//8, 256//8, 256//8]
                                ,([0, 256] + [0, 256] + [0, 256]))
        return normhist(sum(map(h, xs)))
    
    #测量球的位置
    def smooth(s, x):
        return cv.GaussianBlur(x, (3, 3), s)
        #return gaussian_filter(x, s, mode='constant')
    
    #把球的视频读进来
    cap = cv.VideoCapture("bb.mp4")
    #打印一些信息
    print(cap.get(cv.CAP_PROP_FPS))
    #做背景消除，可以提取运动物体出来
    fgbg = cv.createBackgroundSubtractorMOG2(500, 60, True)
    kernel = np.ones((3, 3), np.uint8)
    crop = False
    camshift = False
    pause = False
    # camshift的终止条件
    termination = (cv.TERM_CRITERIA_EPS | cv.TERM_CRITERIA_COUNT, 10, 1)
    
    # 30 25
    fps = int(cap.get(cv.CAP_PROP_FPS))#30 #120
    dt = 1 / fps
    
    
    # x,y,vx,vy  x坐标，y坐标，线速度，角速度；
    
    # x mu：跟踪物体机器人的状态，x: 位置,y： 位置,vx：v方向的速度,vy：y方向的速度
    # 状态矩阵 顺序分别为x,y,vx,vy ；   x = np.array([0, 0, 0, 0])
    mu = np.array([0, 0, 0, 0])# 状态矩阵 4行1列
    # F :状态转义矩阵 它与mu相成计算出预测状态
    '''
    F : 状态转义矩阵
    x  = x  + vx*t + 1/2 * ax * t^2 == 1*x + 0*y + t*vx + 0*vy    + 1/2 * t^2 * ax     【1,0，dt，0】（后面的加速度  1/2 * ax * t^2 单独在B中处理）
    y  = y  + vy*t + 1/2 * ay * t^2 == 0*x + 1*y + 0*vx + t*vy    + 1/2 * t^2 * ay     【0,1，dt，0】（后面的加速度  1/2 * ay * t^2 单独在B中处理）
    vx = vx + ax * t                == 0*x + 0*y + 1*vx + 0*vy    +  t * ax            【0,0，1，0】（后面的加速度   ax * t 单独在B中处理）
    vy = vy + ay * t                == 0*x + 0*y + 0*vx + 1*vy    +  t * ay          【0,0，0，1】（后面的加速度   ay * t 单独在B中处理）
    '''
    F = np.array(
        [1, 0, dt, 0,
         0, 1, 0, dt,
         0, 0, 1, 0,
         0, 0, 0, 1]).reshape(4, 4)
    
    # 控制 将输入转换矩阵，B：控制向量u的转换矩阵
    '''
    B，u：控制方程外部输入的控制 不是必须的，比如外部加速度
    1/2 * t^2 * ax ==  1/2 * t^2 * ax + 0*ay
    1/2 * t^2 * ay ==  0*ax           + 1/2 * t^2 * ay + 
    t * ax == t * ax + 0 * ay
    t * ay == 0 * ax + t * ay
    '''
    # B转成4*2的矩阵 ， 因为 mu 状态矩阵是4*1的 B*U也必须与mu一致是4*1的矩阵；u是2*1的 所以B需要做成4*2的矩阵，B*U就转成4*1的矩阵 4*2 与 2*1  转成 4*1
    B = np.array(
        [dt ** 2 / 2, 0,
         0, dt ** 2 / 2,
         dt, 0,
         0, dt]).reshape(4, 2)
    
    '''
    也可以
    B = np.array(
        [0.5*dt ** 2, 0,
         0, 0.5*dt ** 2 ,
         dt, 0,
         0, dt]).reshape(4, 2)
    '''
    
    
    # 输入的控制向量 x方向加速度，y方向加速度；每秒多少像素
    #B*U 转成 4*1的矩阵    ；4*2 与 2*1 想成 转成 4*1
    u = np.array([0, 25])# 2*1
    
    # P: 预测的方差,这个值会在卡尔曼滤波的过程中更新
    # 协方差矩阵  ，跟踪4个值  x坐标方差，y坐标方差，线速度方差； 角速度方差
    P = np.diag([1000, 1000, 1000, 1000]) ** 2
    
    # 过程噪声 在预测时,系统产生的过程噪声. 例如我们并不准确知道小车当前速度 （默认影响很小 0.0001）
    Q = 0.0001 ** 2 * np.eye(4)
    
    # 将预测矩阵转换成测量矩阵的样子； 这里测量x的值与y的值，不测量速度   ||  只测量一个位置的话就是1,0，如果位置和速度都测量的话就用1,1;
    # 通过看测量值z的值是z=np.array([xo, yo]) 测x与y的值  z:2*1来确定H的取值；
    H = np.array(
        [1, 0, 0, 0,
         0, 1, 0, 0]).reshape(2, 4)
    
    # 测量噪声  R: 测量噪声,通常情况下,测量设备不变,那么测量噪声的值也不会发生变化
    R = 9 ** 2 * np.eye(2)
    
    # res = [(mu,P,mu)]
    res = []
    N = 15
    
    
    # 保存卡尔曼滤波数据
    listCenterX = []
    listCenterY = []
    listpoints = []
    
    count=0
    
    import time
    
    #读视频内容
    ret, frame = cap.read()
    
    while ret:
    
        start = time.time()
    
        key = cv.waitKey(1)
        # 程序按ESC退出
        if key == 27: break
        # 读取摄像头数据
        ret, frame = cap.read()
        # 若读取失败则退出
        if not ret: break;
        # 背景消除找到运动的物品
        bgs = fgbg.apply(frame)
        # 去除多余的一些小噪点
        bgs = cv.erode(bgs, kernel, iterations=1)
        bgs = cv.medianBlur(bgs, 3)
        bgs = cv.dilate(bgs, kernel, iterations=2)
        cv.imshow("mask", bgs)
    
        # 比较小的话改大一点
        bgs[bgs>200] = 255
        # 从彩图中截取出运动的物品
        colorMask = cv.bitwise_and(frame, frame, mask=bgs);
        # 截图
        if not crop:
            fromCenter = False
            img = frame
            # r = cv.selectROI(frame,fromCenter)
            #手动写死的值，把球截出来
            r = [284, 319, 39, 44]
            imCrop = img[int(r[1]):int(r[1] + r[3]), int(r[0]):int(r[0] + r[2])]
    
            crop = True
            camshift = True
            imCropMask = cv.cvtColor(imCrop, cv.COLOR_BGR2GRAY)
            ret, imCropMask = cv.threshold(imCropMask, 30, 255, cv.THRESH_BINARY)
            his = smooth(1, rgbh([imCrop], imCropMask))
            cv.imshow("aa", imCropMask)
            roiBox = (int(r[0]), int(r[1]), int(r[2]), int(r[3]))
            cv.destroyWindow("ROI selector")
    
        count+=1
    
    
        if camshift and count>90 and count<700:
            cv.putText(frame, "Center roiBox", (0, 10), cv.FONT_HERSHEY_SIMPLEX, 0.5, (0, 255, 0), 2, cv.LINE_AA)
            cv.putText(frame, "Estimatedposition", (0, 30), cv.FONT_HERSHEY_SIMPLEX, 0.5, (255, 255, 0), 2, cv.LINE_AA)
            cv.putText(frame, "Prediction", (0, 50), cv.FONT_HERSHEY_SIMPLEX, 0.5, (0, 0, 255), 2, cv.LINE_AA)
    
            rgbr = np.floor_divide(colorMask, 8)
            r, g, b = rgbr.transpose(2, 0, 1)
            #拿到rgb的数据
            l = his[r, g, b]
            maxl = l.max()
            # 跟踪出来是返回结果是椭圆，跟踪这个球，目的是跟踪球的球心
            (rb, roiBox) = cv.CamShift(l, roiBox, termination)
            cv.ellipse(frame, rb, (0, 255, 0), 2)
            # 感兴趣区域的中心，得到球的中心
            xo = int(roiBox[0] + roiBox[2] / 2)
            yo = int(roiBox[1] + roiBox[3] / 2)
    
            #把它绘制出来
            cv.circle(frame, (int(xo), int(yo)), int(5), (255, 0, 255), -1)
    
    
            isFindBall = False
            # 找出来的东西太小了
            if (bgs.sum() < 50):
                #开始卡尔曼滤波
                mu, P = kalman(mu, P, F, Q, B, u, None, H, R)
                m = "None"
                isFindBall = False
                print("没有检测到")
            else:
                # 通过看测量值z的值是z=np.array([xo, yo]) 测x与y的值  z:2*1来确定H的取值；
                # 开始卡尔曼滤波  z:  np.array([xo, yo]) 测量值 （把这里测量球的圆心传进去）
                z=np.array([xo, yo])   # z:2*1  ==> H:2*4 @ mu:4*1
                mu, P = kalman(mu, P, F, Q, B, u,z, H, R) #返回滤波后的值与协方差
                m = "normal"
                isFindBall = True
    
            if (isFindBall):
                # 保存卡尔曼滤波数据
                listCenterX.append(xo)
                listCenterY.append(yo)
            # 若找到了,则将找到的东西保存起来
            listpoints.append((xo, yo, m))# 保存卡尔曼滤波数据
            res += [(mu, P)]
    
    
    
            # 绘制实时的圆形
            cv.circle(frame, (int(xo), int(yo)), int(5), (255, 0, 255), -1)
    
            # 向后去预测
            mu2 = mu
            P2 = P
            res2 = []
            # 向后预测两倍帧率的时间轨迹
            for _ in range(fps * 5):
                # 向后预测 z（圆心）为None
                mu2, P2 = kalman(mu2, P2, F, Q, B, u, None, H, R)
                res2 += [(mu2, P2)]
            # 取出xy的卡尔曼滤波结果,以及他们的协方差
            xe = [mu[0] for mu, _ in res]
            xu = [np.sqrt(P[0, 0]) for _, P in res]
            ye = [mu[1] for mu, _ in res]
            yu = [np.sqrt(P[1, 1]) for _, P in res]
    
            # 取出向后预测的xy的位置和协方差
            xp = [mu2[0] for mu2, _ in res2]
            yp = [mu2[1] for mu2, _ in res2]
            xpu = [np.sqrt(P[0, 0]) for _, P in res2]
            ypu = [np.sqrt(P[1, 1]) for _, P in res2]
    
            # 绘制实时跟踪
            for n in range(len(listCenterX)):
                cv.circle(frame, (int(listCenterX[n]), int(listCenterY[n])), 3, (0, 255, 0), -1)
    
            # 绘制卡尔曼滤波结果
            for n in range(len(xe)):
                radius = (xu[n] + yu[n]) / 2
                cv.circle(frame, (int(xe[n]), int(ye[n])), int(radius), (255, 255, 0), 1)
    
            # 绘制预测的轨迹
            for n in range(len(xp)):
                radius = (xpu[n] + ypu[n]) / 2
                cv.circle(frame, (int(xp[n]), int(yp[n])), int(radius), (0, 0, 255))
    
            #判断当球碰到地面之后，需要重新计算kalmanFilter
            # 判断速度是否发生了突变  |  listCenterY[-1] > 510：代表球碰地了，屏幕有高度，设置 510是屏幕的y像素（高度）阈值 y0>510;listCenterY[-1] ==y0
            # 重新初始化
            if (len(listCenterY) > 4):
                if ((listCenterY[-5] < listCenterY[-4]) and (listCenterY[-4] < listCenterY[-3])
                        and (listCenterY[-3] > listCenterY[-2]) and (listCenterY[-2] > listCenterY[-1])) or listCenterY[-1] > 510:
                    print("===========================重置===========================",listCenterY[-1])
                    listCenterY = []
                    listCenterX = []
                    listpoints = []
                    res = []
                    mu = np.array([0, 0, 0, 0])
                    P = np.diag([100, 100, 100, 100]) ** 2
    
    
    
        cv.imshow("mask", colorMask)
        cv.imshow("Frame", frame)

[2021060907043725.png]: /images/20221005/76663bd58fde41aea9aa4ec429a4ab25.png
[watermark_type_ZmFuZ3poZW5naGVpdGk_shadow_10_text_aHR0cHM6Ly9ibG9nLmNzZG4ubmV0L3FxXzE1MjA0MTc5_size_16_color_FFFFFF_t_70]: /images/20221005/6538d82771d24a2daf49f4f9fd1812c0.png