基于卡尔曼滤波的信号处理以及图像追踪Python实现

快速上手卡尔曼滤波，使用Python描述简单的卡尔曼信号处理，以及在CV中进行目标追踪。

叫我田小霖啦

10482人浏览 · 2022-04-28 21:10:42

叫我田小霖啦 · 2022-04-28 21:10:42 发布

引言

这篇文章将由浅入深，从最简单的信号处理开始，到计算机视觉图像跟踪的应用。该文章使用Python语言，在进行视觉处理时，并未使用opencv自带的api，而是从矩阵运算进行逐步处理，更易于理解。

卡尔曼滤波

原理

网上现在能看到的原理解释都很详细，这里就不多介绍。本人理解卡尔曼滤波的本质就是最优估计，通过实际观测与卡尔曼估计进行加权。这里推荐我看到的比较好的两篇文章。CSDNhttps://blog.csdn.net/codesamer/article/details/81191487

知乎https://zhuanlan.zhihu.com/p/48876718

在大多数的卡尔曼滤波的原理介绍都是使用小车运动的例子。接下来将使用其他案例代码为大家讲解如何使用这项技术。

关键公式

上图就是卡尔曼滤波的关键公式，包括两个状态预测公式（4.9，4.10）和三个参数更新公式（4.11,4.12,4.13）。

卡尔曼滤波实战

一维数据

代码及效果

# 开发作者   ：Tian.Z.L
# 开发时间   ：2022/4/23  21:17 
# 文件名称   ：test.PY
# 开发工具   ：PyCharm
import numpy as np
import matplotlib.pyplot as plt


# 定义x的初始状态
x_mat = np.mat([[0, ], [0, ]])
# 定义初始状态协方差矩阵
p_mat = np.mat([[1, 0], [0, 1]])
# 定义状态转移矩阵，因为每秒钟采一次样，所以delta_t = 1
f_mat = np.mat([[1, 1], [0, 1]])
# 定义状态转移协方差矩阵，这里我们把协方差设置的很小，因为觉得状态转移矩阵准确度高
q_mat = np.mat([[0.0001, 0], [0, 0.0001]])
# 定义观测矩阵
h_mat = np.mat([1, 0])
# 定义观测噪声协方差
r_mat = np.mat([1])

# array = np.array([50] * 200)
array = np.array(np.arange(0, 7, 0.01))
array = np.array(np.sin(array))
array *= 3
s = np.random.normal(0, 0.5, 700)
test_array = array + s
plt.plot(test_array, marker='.', linestyle='None', color='blue')
after = []
for i in range(700):
    x_predict = f_mat * x_mat
    p_predict = f_mat * p_mat * f_mat.T + q_mat
    kalman = p_predict * h_mat.T / (h_mat * p_predict * h_mat.T + r_mat)
    x_mat = x_predict + kalman * (test_array[i] - h_mat * x_predict)
    p_mat = (np.eye(2) - kalman * h_mat) * p_predict
    after.append(x_mat[0, 0])


plt.plot(after, color='red')
plt.show()

代码讲解

如果您并不想过多的掌握卡尔曼滤波背后的数学处理原理和方法等，那么接下来的部分将会快速的带您初步掌握以及将卡尔曼滤波移植到自己的项目中。相比于小车案例，这里可以类比为间隔时间恒定的小车运动。时间间隔为横坐标，小车的行使距离P就是纵坐标。

根据卡尔曼滤波的5个关键公式，需要初始化这些矩阵，但是这些矩阵并不是都需要自己编写。在以上代码中，您需要更改的是f_mat、h_mat、q_mat。接下来讲讲如何更改。

x_mat为卡尔曼最优估计结果矩阵。我们所需要的滤波之后的数据就存在这个矩阵中。
p_mat为状态协方差矩阵。这个矩阵不需要进行更改。
f_mat为状态转移矩阵。这个矩阵的数值是根据观测数据的关系进行赋值的。例如在小车示例中，如果x_mat为[[P],[V]]那么状态转移矩阵就为[[1,1],[0,1]]。反之就调换一下。

q_mat为状态转移协方差矩阵。这个矩阵在对角线的值将会影响卡尔曼滤波的效果。左侧为数值0.03所形成的效果图。右图为0.0001

h_mat为观测矩阵。这个矩阵决定了x_mat矩阵里面数字的有效性。这里设置为监督位置的信息。
r_mat为噪声的协方差矩阵。不需要修改。

注意：f_mat矩阵的改变，您也需要改变h_mat。例如，将上述的P和V的位置上下对调。那么此时x_mat对应的反应P的数值就为x_mat[1,0]而不是x_mat[0,0]。对应的观测矩阵h_mat也需要修改。

在这个案例中，所使用的是对于sin正弦函数的一个滤波。首先是将0-7分为700份，然后生成了3SinX+s的数组。X的范围为[0,700)，s为高斯噪声均值为0，方差为0.5。使用matplotlib画出来的就为上面图显示的蓝色散点。

下面的代码就是真正的进入到卡尔曼滤波了。这五行代码就是根据那五个式子写的。需要注意的地方就是根据f_mat和h_mat的设置的不同。所需要的数据在x_mat的不同位置。

二维数据

简单版本的代码及效果

# 开发作者   ：Tian.Z.L
# 开发时间   ：2022/4/26  9:52 
# 文件名称   ：HandTrackingByKalman.PY
# 开发工具   ：PyCharm
import numpy as np
import mediapipe as mp
import cv2

print(cv2.useOptimized())
mpHands = mp.solutions.hands
hands = mpHands.Hands(static_image_mode=False,
                      max_num_hands=2,
                      min_detection_confidence=0.5,
                      min_tracking_confidence=0.5)
# 定义x的初始状态 -- 需要修改，初始化为捕获手势的位置x坐标
x_mat = np.mat([[0, ], [0, ]])
y_mat = np.mat([[0, ], [0, ]])
# 定义初始状态协方差矩阵
p_x_mat = np.mat([[1, 0], [0, 1]])
p_y_mat = np.mat([[1, 0], [0, 1]])
# 定义初始化控制矩阵
b_mat = np.mat([[0.5, ], [1, ]])
# 定义状态转移矩阵，因为每秒钟采一次样，所以delta_t = 1
f_mat = np.mat([[1, 1], [0, 1]])
# 定义状态转移协方差矩阵，这里我们把协方差设置的很小，因为觉得状态转移矩阵准确度高
q_mat = np.mat([[0.03, 0], [0, 0.03]])
# 定义观测矩阵
h_mat = np.mat([1, 1])
# 定义观测噪声协方差
r_mat = np.mat([1])

video = cv2.VideoCapture('output.avi')
first_frame_flag = True
while True:
    ret, frame = video.read()
    img = cv2.cvtColor(frame, cv2.COLOR_BGR2RGB)
    results = hands.process(img)
    h, w, c = img.shape
    if results.multi_hand_landmarks:
        for handLms in results.multi_hand_landmarks:
            for id, lm in enumerate(handLms.landmark):
                cx, cy = int(lm.x * w), int(lm.y * h)
                if id == 9:
                    if first_frame_flag:
                        x_mat[0, 0] = cx
                        y_mat[0, 0] = cy
                        first_frame_flag = False
                    else:
                        x_predict = f_mat * x_mat
                        y_predict = f_mat * y_mat
                        p_x_predict = f_mat * p_x_mat * f_mat.T + q_mat
                        p_y_predict = f_mat * p_y_mat * f_mat.T + q_mat
                        kalman_x = p_x_predict * h_mat.T / (h_mat * p_x_predict * h_mat.T + r_mat)
                        kalman_y = p_y_predict * h_mat.T / (h_mat * p_y_predict * h_mat.T + r_mat)
                        x_mat = x_predict + kalman_x * (cx - h_mat * x_predict)
                        y_mat = y_predict + kalman_y * (cy - h_mat * y_predict)
                        p_x_mat = (np.eye(x_mat.shape[0]) - kalman_x * h_mat) * p_x_predict
                        p_y_mat = (np.eye(y_mat.shape[0]) - kalman_y * h_mat) * p_y_predict
                        noise_x = cx + int(np.random.normal(0, 1) * 10)
                        noise_y = cy + int(np.random.normal(0, 1) * 10)
                        cv2.circle(frame, (noise_x, noise_y), 6, (0, 0, 255), cv2.FILLED)
                        # cv2.circle(frame, (int(x_mat[0, 0]), int(y_mat[0, 0])), 3, (0, 255, 0), cv2.FILLED)
                        cv2.circle(frame, (int(x_mat[0, 0]), int(y_mat[0, 0])), 3, (255, 0, 0), cv2.FILLED)
                    break
    cv2.imshow('video', frame)
    if cv2.waitKey(20) & 0xFF == ord('q'):
        break
cv2.destroyAllWindows()
video.release()

蓝色点为卡尔曼最佳推断

红点为当前观察值点

简单版本的代码讲解

在原有的基础上，只是在一个维度上（只考虑距离和时间的线性关系）进行，在图像目标跟踪上，最基础的就是目标的x,y两个坐标。这里称为二维数据的处理。类比于小车的坐标与时间的这个例子，这里进行目标追踪，在帧数基本不变的情况下，分别对x以及对y的处理在思想上是一样的。因此在处理目标跟踪时，不妨对目标进行两次卡尔曼处理。这也就是为什么称为简单版本。

可以很明显的观察到，上面的效果图是追踪我的手掌。这里使用的是mediapipe库，这个库具有强大的识别能力，在姿态识别，手势识别，面部识别等很多检测具有强大的用处。mediapipe

的速度非常快，用CPU就已经能达到非常高的检测速度。详细了解这个库的用法可以去搜索，也可以查看低成本Hand Tracking机械臂手势识别与跟踪这篇博客，有简单的提到。

根据前文提到的卡尔曼的5个关键公式可以知道，在进行参数更新的时候，会更新最优估计矩阵和状态估计协方差矩阵。对于x、y来说，他们是相互独立的。因此这里对x、y分别初始化了矩阵。下面进行卡尔曼滤波的操作就跟一维的没什么不同了。只是分别进行了两次不同维度的卡尔曼滤波。

此时问题就出现了，虽然很容易理解，但是在目标跟踪里，很多情况下除了x、y这样最基础的坐标之外，往往还有长宽等。因此对于这种每多一个需要监督的数据，就要多写一遍卡尔曼滤波代码，看起来十分的冗余，此时，我们需要对这种情况进行整合，把所有需要监督的数据放在一个矩阵中，这样只需要执行一遍卡尔曼滤波代码就能得到所有需要的数据。

复杂版本的代码及效果

# 开发作者   ：Tian.Z.L
# 开发时间   ：2022/4/26  16:32 
# 文件名称   ：HandTrackingByKalman2.PY
# 开发工具   ：PyCharm
import numpy as np
import mediapipe as mp
import cv2
import matplotlib.pyplot as plt

mpHands = mp.solutions.hands
hands = mpHands.Hands(static_image_mode=False,
                      max_num_hands=2,
                      min_detection_confidence=0.5,
                      min_tracking_confidence=0.5)

# 定义x的初始状态 -- 需要修改，初始化为捕获手势的位置x坐标
x_mat = np.mat([[0, ], [0, ], [0, ], [0, ]])
# 定义初始状态协方差矩阵
p_mat = np.mat([[1, 0, 0, 0], [0, 1, 0, 0], [0, 0, 1, 0], [0, 0, 0, 1]])
# 定义初始化控制矩阵
b_mat = np.mat([[0.5, ], [1, ], [0.5, ], [1, ]])
# 定义状态转移矩阵，因为每秒钟采一次样，所以delta_t = 1
f_mat = np.mat([[1, 1, 0, 0], [0, 1, 0, 0], [0, 0, 1, 1], [0, 0, 0, 1]])
# 定义状态转移协方差矩阵，这里我们把协方差设置的很小，因为觉得状态转移矩阵准确度高
q_mat = np.mat([[0.3, 0, 0, 0], [0, 0.3, 0, 0], [0, 0, 0.3, 0], [0, 0, 0, 0.3]])
# 定义观测矩阵
h_mat = np.mat([1, 0, 1, 0])
# 定义观测噪声协方差
r_mat = np.mat([1])

video = cv2.VideoCapture('output.avi')
first_frame_flag = True
while True:
    ret, frame = video.read()
    img = cv2.cvtColor(frame, cv2.COLOR_BGR2RGB)
    results = hands.process(img)
    h, w, c = img.shape
    if results.multi_hand_landmarks:
        for handLms in results.multi_hand_landmarks:
            for id, lm in enumerate(handLms.landmark):
                cx, cy = int(lm.x * w), int(lm.y * h)
                if id == 9:
                    if first_frame_flag:
                        x_mat[0, 0] = cx
                        x_mat[2, 0] = cy
                        first_frame_flag = False
                    else:
                        x_predict = f_mat * x_mat
                        p_predict = f_mat * p_mat * f_mat.T + q_mat
                        x_mat_before = x_mat
                        kalman = p_predict * h_mat.T / (h_mat * p_predict * h_mat.T + r_mat)
                        # 出现问题
                        x_mat = x_predict + np.multiply(kalman, (np.mat([[cx, ], [cx - x_mat_before[0, 0], ], [cy, ], [cy - x_mat_before[2, 0], ]]) - x_predict))
                        p_mat = (np.eye(x_mat.shape[0]) - kalman * h_mat) * p_predict
                        noise_x = cx + int(np.random.normal(0, 1) * 10)
                        noise_y = cy + int(np.random.normal(0, 1) * 10)
                        cv2.circle(frame, (noise_x, noise_y), 6, (0, 0, 255), cv2.FILLED)
                        cv2.circle(frame, (int(x_mat[0, 0]), int(x_mat[2, 0])), 3, (0, 255, 0), cv2.FILLED)
                    break
    cv2.imshow('video', frame)
    if cv2.waitKey(33) & 0xFF == ord('q'):
        break
cv2.destroyAllWindows()
video.release()

红色的点为实际观测点

绿点为卡尔曼最佳估计点

复杂版本的讲解

在进行卡尔曼滤波之前。我们需要进行矩阵的搭建，这是保证整个流程正确性关键的部分。

根据这一时刻该变量与上一时刻该变量的关系列出状态转移矩阵f_mat、最优估计矩阵x_mat。剩余的其他矩阵可以根据上面简单的示例举一反三的列出来。在卡尔曼滤波的过程中，有以下问题需要注意。

x_predict = f_mat * x_mat
p_predict = f_mat * p_mat * f_mat.T + q_mat
x_mat_before = x_mat
kalman = p_predict * h_mat.T / (h_mat * p_predict * h_mat.T + r_mat)
# 出现问题
x_mat = x_predict + np.multiply(kalman, (
            np.mat([[cx, ], [cx - x_mat_before[0, 0], ], [cy, ], [cy - x_mat_before[2, 0], ]]) - x_predict))
p_mat = (np.eye(x_mat.shape[0]) - kalman * h_mat) * p_predict

跟上面只对一个维度的数据不同，这里监督的两个维度的数据。在上面的第五行代码，也是就注释下面的代码是需要进行修改的。

如果选择的观测矩阵h_mat为[1,0,1,0]的话，意思为只观测x、y位置，此时在更新x_mat的时候可以不需要考虑x、y方向的速度。可以用以下的代码代替。当然用上面的代码也不会有问题。

x_mat = x_predict + np.multiply(kalman, (np.mat([[cx, ], [0, ], [cy, ], [0, ]]) - x_predict))

2.如果选择的观测矩阵h_mat为[1,1,1,1]的话，意思为观测x的位置、x方向的速度以及y的位置、y方向的速度。那么如何得到速度呢？由于在帧数差不多的情况下，每一帧的间隔是大致相同的。那么不妨设每一帧的间隔为一个单位时间，那么通过对比前后两帧的距离，就可以得到速度。这就是上面代码描述的事情。

3.x_mat在进行参数更新的时候所使用的方法也是不一样的。初始的卡尔曼滤波在这一步采用的是使用上一步得到的卡尔曼系数乘以观测矩阵和卡尔曼推断的差值。在进行这一步的时候，x_predict的shape为4*1，kalman的shape为4*1。

因为x方向和y方向是相互独立的，如果按照卡尔曼滤波的方法，那么得到的观测矩阵和卡尔曼推断的差值是1*1的矩阵:[h_mat(1*4)*实际观测量(4*1) - h_mat(1*4)*x_preadict(4*1)]。得到的值在与卡尔曼系数kalman(4*1)相乘，得到的值是线性的，即x方向与y方向得到的值是线性关系，并不是相互独立的。