Kalman Filter

发表于 | 分类于 | 阅读次数：

1. 离散卡尔曼滤波

待估计的过程

卡尔曼滤波解决的是离散控制过程状态估计问题，状态$x \in \Re^{n}$， 系统由线性随机差分方程控制：

同时观测值$z \in \Re^{m}$表示为：

式中随机变量$w_{k-1}$和$v_k$表示为系统运行误差和测量误差，假设均为白噪声正态分布：

过程噪声方差$Q$和测量噪声方差$R$ 认为不变。$n \times n$ 的矩阵$A$表示在不考虑控制函数和噪声的情况下，上一个时刻的状态与当前时刻状态之间的关系，实际问题中矩阵$A$可能是变化的，但是此处假设是不变的。$n \times l$ 的矩阵 $B$ 表示表示控制输入$u \in \Re^{l}$ 与状态之间的关心， $m \times n$ 的矩阵 $H$ 是测量矩阵。

滤波器的计算基础

定义系统的第$k$ 步的先验状态估计值为$\hat{x}_{k}^{-} \in \Re^{n}$ ，定义给定测量值$z_k$之后的系统后验估计状态为$\hat{x}_{k} \in \Re^{n}$ ，此时可以定义先验估计误差和后验估计误差分别为：

先验估计方差和后验估计方差分别定义为：

定义后验观测值的表达式为：

$(z_k-H\hat{x}_k^-)$ 称作测量残差，表示实际测量值与测量估计值的差值， $n\times m$ 维矩阵$K$ 定义为 Blending factor，一般通过最大化后验估计方差来求解。具体计算方式为，首先根据后验观测值表达式得出每一步估计的后验，然后根据后验表达误差计算出$e_k$, 再通过期望估计计算出$P_k$, 求$P_k$对$K$的导数，并使导数为0解出$K$。最小二乘解为：

由此可看出，当测量误差方差$R$ 趋近于0时，因子$K$ 趋近于测量矩阵的逆矩阵：

如果先验估计误差方差$P_{k}^{-}$趋近于0，则因子$K$ 就趋近于0。解释意义为：当测量误差方差小的时候，测量结果更可信，当先验估计方差小的时候，预计测量值更加可信。

滤波器的概率基础

从概率的角度来说，滤波器的后验可以通过贝叶斯公式重新描述，卡尔曼滤波保留了状态分布的前两阶矩，一阶矩是期望，二阶矩是方差，即：

后验状态估计等于状态分布的均值，后验估计误差方差代表状态分布的方差，也就是说：

离散卡尔曼滤波算法

首先从广义的角度解释卡尔曼滤波器，后面章节会从具体的角度解读。Kalmam滤波器利用反馈控制的方式估计系统：滤波器首先估计系统的状态，然后获得带有误差的测量反馈。Kalman滤波的公式分成两部分，时间更新和测量更新，时间更新部分通过当前状态和当前状态的误差方差估计下一个状态的先验，测量更新是指将测量结果加入到先验估计中来获取后验分布。

时间更新环节也可以当作是一种预测环节，测量更新环节可以当作是一种误差修正，因此整体估计算法就是一种预测-修正迭代流程，时间更新表示为：

其中方差更新可以根据上面的式子计算的。

测量更新分三部分，包括因子矩阵计算，后验状态估计，后验状态方差：

因子矩阵计算就是按照上面的最小二乘解，后验状态方差我就不知道为什么了。

2. Coding

定义Kalman滤波类，包含两个函数，预测函数和更新函数。

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
from np.linalg import inv
from numpy import identity
class KalmanFilter:
    def __init__(self, X, P, F, Q, Z, H, R):
        """
        X: State Estimate
        P: State Estimate Covaiance 
        F: Stage transition model
        Q: Process noise covariance
        Z: Measurement of X
        H: Observation model
        R: Observation noise covariance
        """
        self.X = X
        self.P = P
        self.F = F
        self.Q = Q
        self.Z = Z
        self.H = H
        self.R = R
        
	def predict(self, X, P, w=0):
        X = self.F * X + w
        P = self.F * P * self.F.T + self.Q
        return X, P
    
    def update(self, X, P, Z):
        K = P * self.H.T * inv(self.H * P * self.H.T + self.R)
        X = X + K * (Z - self.H * X)
        P = (identity(P.shape[1]) - K * self.H) * P
        return X, P