一. 简介

它是啥？
卡尔曼滤波（Kalman filtering）是一种利用线性系统状态方程，通过系统输入输出观测数据，**对系统状态进行最优估计的算法。**由于观测数据中包括系统中的噪声和干扰的影响，所以最优估计也可看作是滤波过程。（来自百度百科）
能干嘛？
一句话：适用于估计一个动态系统的最优状态。（单单看这一句可能有些抽象，后面主要结合代码进行说明）
怎么实现？
直接上公式：
（图片来自：http://bilgin.esme.org/BitsAndBytes/KalmanFilterforDummies）

二. OpenCV里的kalman-filter

地址：戳这

主要看三部分：init、predict、correct

init

void KalmanFilter::init(int DP, int MP, int CP, int type)
{
    
    
    CV_Assert( DP > 0 && MP > 0 );
    CV_Assert( type == CV_32F || type == CV_64F );
    CP = std::max(CP, 0);

    statePre = Mat::zeros(DP, 1, type);
    statePost = Mat::zeros(DP, 1, type);
    transitionMatrix = Mat::eye(DP, DP, type);

    processNoiseCov = Mat::eye(DP, DP, type);
    measurementMatrix = Mat::zeros(MP, DP, type);
    measurementNoiseCov = Mat::eye(MP, MP, type);

    errorCovPre = Mat::zeros(DP, DP, type);
    errorCovPost = Mat::zeros(DP, DP, type);
    gain = Mat::zeros(DP, MP, type);

    if( CP > 0 )
        controlMatrix = Mat::zeros(DP, CP, type);
    else
        controlMatrix.release();

    temp1.create(DP, DP, type);
    temp2.create(MP, DP, type);
    temp3.create(MP, MP, type);
    temp4.create(MP, DP, type);
    temp5.create(MP, 1, type);
}

这部分主要是初始化，各个变量意义如下所示，同时结合上、下图的公式理解：

statePre：卡尔曼滤波器更新前的状态
statePost：卡尔曼滤波器更新后的状态
transitionMatrix：状态转移矩阵A
processNoiseCov：过程噪声协方差矩阵
measurementMatrix：测量矩阵H
measurementNoiseCov：测量噪声协方差矩阵
errorCovPre：更新前的P`
errorCovPost：更新后的P
gain：卡尔曼增益K
controlMatrix：控制矩阵B

在这里插入图片描述

predict

const Mat& KalmanFilter::predict(const Mat& control)
{
    
    
    CV_INSTRUMENT_REGION();

    // update the state: x'(k) = A*x(k)
    statePre = transitionMatrix*statePost;

    if( !control.empty() )
        // x'(k) = x'(k) + B*u(k)
        statePre += controlMatrix*control;

    // update error covariance matrices: temp1 = A*P(k)
    temp1 = transitionMatrix*errorCovPost;

    // P'(k) = temp1*At + Q
    gemm(temp1, transitionMatrix, 1, processNoiseCov, 1, errorCovPre, GEMM_2_T);

    // handle the case when there will be measurement before the next predict.
    statePre.copyTo(statePost);
    errorCovPre.copyTo(errorCovPost);

    return statePre;
}

从代码中可以看到，predict实现的是第一幅图中左侧公式的功能，把statePre的值赋给statePost（为了后续correct的操作），同时返回statePre。控制噪声的部分往往会取0，不加入计算。

在这里插入图片描述

correct

const Mat& KalmanFilter::correct(const Mat& measurement)
{
    
    
    CV_INSTRUMENT_REGION();

    // temp2 = H*P'(k)
    temp2 = measurementMatrix * errorCovPre;

    // temp3 = temp2*Ht + R
    gemm(temp2, measurementMatrix, 1, measurementNoiseCov, 1, temp3, GEMM_2_T);

    // temp4 = inv(temp3)*temp2 = Kt(k)
    solve(temp3, temp2, temp4, DECOMP_SVD);

    // K(k)
    gain = temp4.t();

    // temp5 = z(k) - H*x'(k)
    temp5 = measurement - measurementMatrix*statePre;

    // x(k) = x'(k) + K(k)*temp5
    statePost = statePre + gain*temp5;

    // P(k) = P'(k) - K(k)*temp2
    errorCovPost = errorCovPre - gain*temp2;

    return statePost;
}

correct部分，主要实现的是上图中右侧公式的功能，如下图所示。更新了卡尔曼增益gain（公式中的K），statePost（x_k）以及errorCovPost（P_k），最后返回statePost。
在这里插入图片描述

三. 一个例子：kalman.py

#!/usr/bin/env python
"""
   Tracking of rotating point.
   Rotation speed is constant.
   Both state and measurements vectors are 1D (a point angle),
   Measurement is the real point angle + gaussian noise.
   The real and the estimated points are connected with yellow line segment,
   the real and the measured points are connected with red line segment.
   (if Kalman filter works correctly,
    the yellow segment should be shorter than the red one).
   Pressing any key (except ESC) will reset the tracking with a different speed.
   Pressing ESC will stop the program.
"""
# Python 2/3 compatibility
import sys
PY3 = sys.version_info[0] == 3

if PY3:
    long = int

import cv2 as cv

from math import cos, sin, sqrt
import numpy as np

def main():
    img_height = 500
    img_width = 500
    kalman = cv.KalmanFilter(2, 1, 0)

    code = long(-1)

    cv.namedWindow("Kalman")

    while True:
        state = 0.1 * np.random.randn(2, 1)

        kalman.transitionMatrix = np.array([[1., 1.], [0., 1.]])  # 2x2 A  不更新*******************
        kalman.measurementMatrix = 1. * np.ones((1, 2))  # 1x2 H  不更新*******************
        kalman.processNoiseCov = 1e-5 * np.eye(2)  # 2x2, 过程噪声协方差矩阵 Q 不更新*******************
        kalman.measurementNoiseCov = 1e-1 * np.ones((1, 1))  # 1x1 R  不更新*******************
        kalman.errorCovPost = 1. * np.ones((2, 2))  # 2x2  协方差矩阵 P  更新-------------------
        kalman.statePost = 0.1 * np.random.randn(2, 1)  # 2x1 更新-------------------
        # kalman.gain要更新---------------------

        while True:
            def calc_point(angle):
                return (np.around(img_width/2 + img_width/3*cos(angle), 0).astype(int),
                        np.around(img_height/2 - img_width/3*sin(angle), 1).astype(int))

            state_angle = state[0, 0]
            state_pt = calc_point(state_angle)

            prediction = kalman.predict()
            predict_angle = prediction[0, 0]
            predict_pt = calc_point(predict_angle)

            measurement = kalman.measurementNoiseCov * np.random.randn(1, 1)

            # generate measurement
            measurement = np.dot(kalman.measurementMatrix, state) + measurement  # Z

            measurement_angle = measurement[0, 0]
            measurement_pt = calc_point(measurement_angle)

            # plot points
            def draw_cross(center, color, d):
                cv.line(img,
                         (center[0] - d, center[1] - d), (center[0] + d, center[1] + d),
                         color, 1, cv.LINE_AA, 0)
                cv.line(img,
                         (center[0] + d, center[1] - d), (center[0] - d, center[1] + d),
                         color, 1, cv.LINE_AA, 0)

            img = np.zeros((img_height, img_width, 3), np.uint8)
            draw_cross(np.int32(state_pt), (255, 255, 255), 3)  # xk
            draw_cross(np.int32(measurement_pt), (0, 0, 255), 3)  #zk
            draw_cross(np.int32(predict_pt), (0, 255, 0), 3)  #修正后的xk

            cv.line(img, state_pt, measurement_pt, (0, 0, 255), 3, cv.LINE_AA, 0)
            cv.line(img, state_pt, predict_pt, (0, 255, 255), 3, cv.LINE_AA, 0)

            kalman.correct(measurement)

            process_noise = sqrt(kalman.processNoiseCov[0, 0]) * np.random.randn(2, 1)
            state = np.dot(kalman.transitionMatrix, state) + process_noise  # X
            cv.imshow("Kalman", img)

            code = cv.waitKey(100)
            if code != -1:
                break

        if code in [27, ord('q'), ord('Q')]:
            break

    print('Done')


if __name__ == '__main__':
    print(__doc__)
    main()
    cv.destroyAllWindows()

运行结果如下图所示：
在这里插入图片描述
demo代码分析：

初始化

        state = 0.1 * np.random.randn(2, 1)

        kalman.transitionMatrix = np.array([[1., 1.], [0., 1.]])  # 2x2 A  不更新*******************
        kalman.measurementMatrix = 1. * np.ones((1, 2))  # 1x2 H  不更新*******************
        kalman.processNoiseCov = 1e-5 * np.eye(2)  # 2x2, 过程噪声协方差矩阵 Q 不更新*******************
        kalman.measurementNoiseCov = 1e-1 * np.ones((1, 1))  # 1x1 R  不更新*******************
        kalman.errorCovPost = 1. * np.ones((2, 2))  # 2x2  协方差矩阵 P  更新-------------------
        kalman.statePost = 0.1 * np.random.randn(2, 1)  # 2x1 更新-------------------
        # kalman.gain要更新---------------------

predict，然后转成point格式

            prediction = kalman.predict()
            predict_angle = prediction[0, 0]
            predict_pt = calc_point(predict_angle)

计算measurement，等下correct的时候会用到

            measurement = kalman.measurementNoiseCov * np.random.randn(1, 1)

            # generate measurement
            measurement = np.dot(kalman.measurementMatrix, state) + measurement  # Z

            measurement_angle = measurement[0, 0]
            measurement_pt = calc_point(measurement_angle)

correct

            kalman.correct(measurement)

更新state

            process_noise = sqrt(kalman.processNoiseCov[0, 0]) * np.random.randn(2, 1)
            state = np.dot(kalman.transitionMatrix, state) + process_noise  # X

然后如此循环操作。

四. 总结与需要注意的点

通过代码可以发现，kalman滤波器的predict和correct是对其中的statePre和statePost进行操作的，而不是系统的state，state与measurementMatrix生成measurement（也就是z），measurement传到kalman的correct中实现statePost的更新。
最后系统的state也要记得更新。

结合源码和demo例子去理解kalman滤波器

目录

一. 简介

二. OpenCV里的kalman-filter

三. 一个例子：kalman.py

四. 总结与需要注意的点

猜你喜欢