卡尔曼滤波（Kalman filter）

参考：https://zh.wikipedia.org/wiki/%E5%8D%A1%E5%B0%94%E6%9B%BC%E6%BB%A4%E6%B3%A2

卡尔曼滤波模型：

其中噪声服从

假设初始状态以及每一时刻的噪声{x₀, w₁, ..., w_k, v₁ ... v_k}都互相独立.

卡尔曼滤波器

两个状态变量：

$，$ $在时刻$ $k 的状态的估计$
$，$ $后验估计误差协方差矩阵，度量估计值的精确程度$

预测

$(预测状态)$
$（预测估计协方差矩阵）$

更新

三个辅助变量

$(测量余量）$
$(测量余量协方差)$
$（最优卡尔曼增益）$

$更新滤波器变量 x 与 P ：$

$(更新的状态估计)$
$（更新的协方差估计）$

$以上是kalman的流程（from wiki），下面是具体的推导过程$

$具体推导$

$这边用的是不一样的记号（MIT的lecture）$

然后，我们有

最优卡尔曼增益

上面的就是最优卡尔曼增益，所以有

例子

假设一辆小车，我们值观测到了位置，我们用kalman滤波去估计速度

N = 100
z = np.arange(N) + 0.0 # 观测到的位置，做匀速直线运动， 0， 1， 2， 3，...
noise = np.random.randn(N) # 观测的位置加上1的噪声
z += noise
X = np.array([5, 5]).reshape(-1, 1) #状态初始化
P = np.array([[0, 0], [0, 0]]) # 状态协方差矩阵初始化
F = np.array([[1, 1], [0, 1]]) # 状态转移矩阵
Q = np.array([[0.0001, 0], [0, 0.0001]]) #状态转移协方差矩阵
H = np.array([1, 0]).reshape(1, -1) #观测矩阵
R = 1 # 观测噪声

# 迭代过程
for i in range(N):
    X_ = np.matmul(F, X)
    P_ = np.matmul(np.matmul(F, P), F.transpose()) + Q
    K = np.matmul(
        np.matmul(P_, H.transpose()),
                  np.linalg.inv(
                      np.matmul(np.matmul(H, P_), H.transpose()) + R
                  )
    )
    X = X_ + np.matmul(K, (z[i] - np.matmul(H, X_)))
    P = np.matmul(np.array([[1, 0], [0, 1]]) - np.matmul(K, H), P_)

来源：https://www.cnblogs.com/skykill/p/12000524.html

标签

卡尔曼滤波

矩阵

协方差