计算机视觉-kalmanFilter

1.卡尔曼滤波入门

卡尔曼滤波的引入：

滤波就是将测量得到的波形中的的噪声过滤掉，使得到的数据更趋于真实情况，也更加平滑，方便使用。

卡尔曼滤波适用的系统：

卡尔曼滤波适用线性高斯系统

1.线性系统：满足叠加性和齐次性

叠加性：

齐次性：

2.高斯系统

高斯：噪声满足正态分布

2.卡尔曼滤波

2.1.卡尔曼公式理解

实现过程：使用上一时刻的最优结果预测这一时刻的预测值，同时使用这一时刻观测值（传感器测得的数据）修正这一时刻预测值，得到这一时刻的最优结果

预测：

1.上一时刻的最优估计值，推出这一时刻的预测值：

$x^-t = Fx{t-1} + BU_{t-1}$

2.上一时刻最优估计值方差/协方差和超参数Q推出这一时刻预测值方差/协方差

$P^-t = FP{t-1}F^T + Q$

深入理解，Q其实对应的是过程噪声的方差

更新：

1.这一时刻预测值方差/协方差和超参数R推出卡尔曼增益

$K_t = P^-_tH^T{(HP^-_tH^T+R)}^{-1}$

$K_t$为卡尔曼增益

因为$P_t$是和Q有关的，所以将$P_t^-$的公式带入可以推出卡尔曼增益$K_t$是和Q和R都有关的

深入理解，R其实对应的是观测噪声的方差

2.这一时刻预测值、这一时刻观测值、卡尔曼增益推出这一时刻最优估计值

$x_t = x^-_t + K_t(Z_t - Hx_t^-)$

$Z_t$为这一时刻观测值

3.这一时刻预测值方差/协方差、卡尔曼增益推出这一时刻最优估计值方差/协方差

$P_t = (1-K_tH)P^-_t$

预测更新循环往复，就能得到每一时刻的最优估计值

2.2 举例：

小车有两个状态值，p代表位置，v代表速度。小车处于匀加速直线运动

预测模型：

1.上一时刻的最优估计值，推出这一时刻的预测值：

2.上一时刻最优估计值方差/协方差和超参数Q推出这一时刻预测值方差/协方差

推导：因为

所以

其实第一步中这一时刻预测值后面应该还有一个过程噪声Wt要加上（前面省略了），如下图所示，这里面Q是前面省略的过程噪声Wt的方差

测量模型：

GPS测量只能测量小车的位置，不能测量小车的速度，所以

Zp是小车此时的真实位置

Zv是小车此时的真实速度

Pt是测量得到的小车此时的位置

$\Delta$Pt是GPS测量位置的误差

更新模型：

1.这一时刻预测值方差/协方差和超参数R推出卡尔曼增益$K_t$

前面测量模型中，因为无法测速度，测量相当于是一维的，一维时H看作为1

2.这一时刻预测值、这一时刻观测值、卡尔曼增益推出这一时刻最优估计值

3.这一时刻预测值方差/协方差、卡尔曼增益推出这一时刻最优估计值方差/协方差

预测更新循环往复，就能得到每一时刻的最优估计值

2.3调节超参数

Q和R的取值

当F=1且一维的情况H=1，可以得出：

当我们更信任观测值时，那么应该让卡尔曼增益K增大；从K的公式中可以看出，R越小K越大，Q越大K越大

当我们更信任模型估计值时，那么应该让卡尔曼增益K减小；从K的公式中可以看出，R越大K越小，Q越小K越小

结论：

当我们更信任模型估计值时（模型估计基本没有误差），那么应该让K小一点，我们应该将R取大一点，Q取小点

当我们更信任观测值时（模型估计误差较大），那么应该让K大一点，我们应该将R取小一点，Q取大一点

3.卡尔曼滤波的代码解读

import numpy as np

class KalmanFilter(object):
    def __init__(self, F = None, B = None, H = None, Q = None, R = None, P = None, x0 = None):
      """
      卡尔曼滤波可以认为是中庸算法，将measurements和线性预测值进行综合考虑
      怎么综合考虑呢？卡尔曼滤波算法要动态的调这个比例
      '''
      设状态量有xn个
      - X0为前一时刻状态量，shape=(xn,1)
      - P0为初始状态不确定度， shape=(xn,xn)
      - A为状态转移矩阵，shape=(xn,xn)
      - Q为递推噪声协方差矩阵，shape=(xn,xn)
      - B是外部输入部分
      返回的结果为
      """
        if(F is None or H is None):
            raise ValueError("Set proper system dynamics.")

        self.n = F.shape[1] # self.n = 3
        self.m = H.shape[1] # self.m = 1

        self.F = F
        self.H = H
        self.B = 0 if B is None else B
        self.Q = np.eye(self.n) if Q is None else Q
        self.R = np.eye(self.n) if R is None else R
        self.P = np.eye(self.n) if P is None else P
        self.x = np.zeros((self.n, 1)) if x0 is None else x0

    def predict(self, u = 0):
        '''
        使用上一时刻的最优结果预测这一时刻的预测值
        '''
        #上一时刻的最优估计值，推出这一时刻的预测值：
        #   状态方程x = F * x(t-1) + B*U(t-1)
        #   F：状态转移矩阵，代表当前状态上一时刻的值乘上A这种关系，作用在该系统
        #   x(t-1)：上一个时刻该状态的值；
        #   B：控制矩阵，输入乘上B这种关系，作用在该系统
        #   U:输入，即给到x_{k}的一个输入；
        self.x = np.dot(self.F, self.x) + np.dot(self.B, u)
        
        # P就是协方差矩阵
        # 深入理解，Q其实对应的是过程噪声的方差
        # P = F * P(t-1) * F^T + Q
        self.P = np.dot(np.dot(self.F, self.P), self.F.T) + self.Q
        return self.x

    def update(self, z):
        '''
        使用这一时刻观测值measurement（传感器测得的数据）修正这一时刻预测值，得到这一时刻的最优结果
        '''
        # y = z - H * x 先验估计值
        y = z - np.dot(self.H, self.x)
        # S = H * P * H^T + R
        S = self.R + np.dot(self.H, np.dot(self.P, self.H.T))
        
        # 1.这一时刻预测值方差或者协方差和超参数R推出卡尔曼增益K_{t}
        # K = P * H^T * S^(-1)
        K = np.dot(np.dot(self.P, self.H.T), np.linalg.inv(S))
        
        # 2.这一时刻预测值、这一时刻观测值、卡尔曼增益推出这一时刻最优估计值
        # x(t) = x + K(t) * y ：预测估计+卡尔曼增益*(观测值-先验估计值y)
        self.x = self.x + np.dot(K, y)
        
        # 3.这一时刻预测值方差/协方差、卡尔曼增益推出这一时刻最优估计值方差/协方差
        # P(t) = (I-K*H)*P * (I - K * H^T) + K * R * K^T
        I = np.eye(self.n)
        self.P = np.dot(np.dot(I - np.dot(K, self.H), self.P), 
        	(I - np.dot(K, self.H)).T) + np.dot(np.dot(K, self.R), K.T)

def example():
	dt = 1.0/60
  # F为3 x 3 的矩阵
	F = np.array([[1, dt, 0], [0, 1, dt], [0, 0, 1]])
  # H为1 x 3 的矩阵 [1,0,0]
	H = np.array([1, 0, 0]).reshape(1, 3)
  # Q为3 x 3 的矩阵
	Q = np.array([[0.05, 0.05, 0.0], [0.05, 0.05, 0.0], [0.0, 0.0, 0.0]])
  # R为1 x 1 的矩阵
	R = np.array([0.5]).reshape(1, 1)
  
  # 随机生成-10-10之间的100个随机数字
	x = np.linspace(-10, 10, 100)
  # 观测的值
  # -(x平方 + 2*x - 2) + 创建一个均值为0，方差为2的高斯噪声，共有100个samples
	measurements = - (x**2 + 2*x - 2)  + np.random.normal(0, 2, 100)

	kf = KalmanFilter(F = F, H = H, Q = Q, R = R)
	predictions = []

	for z in measurements:
		predictions.append(np.dot(H,  kf.predict())[0])
		kf.update(z)

	import matplotlib.pyplot as plt
	plt.plot(range(len(measurements)), measurements, label = 'Measurements')
	plt.plot(range(len(predictions)), np.array(predictions), label = 'Kalman Filter Prediction')
	plt.legend()
	plt.show()

if __name__ == '__main__':
    example()