卡尔曼滤波

根据视频https://www.youtube.com/watch?v=2-lu3GNbXM8整理的笔记

汽车在行驶的时候状态的转移
$p_t$表示的是t时刻的位置
$v_t$表示的是t时刻的速度
$u_t$表示的是t时刻的加速度
${\hat{x}}_t^{-}$表示的是预测的t时刻的状态

一维情况下的高斯分布

二维包含噪声的数据在x，y上的投影都是高斯分布，即使两个维度的数据相关，但在坐标轴上的投影与线性无关的数据是一致的
协方差表示两个维度数据的相关性
协方差矩阵对角上的数是方差，反对角线上的数是协方差

上一时刻的协方差矩阵左乘状态转移矩阵右乘状态转移矩阵的转置加上噪声就是当前时刻的协方差矩阵
该公式表示不确定性在各个时刻间的传递关系

H观测矩阵是本身状态到观测状态的转换关系
v表示观测的噪声
R表示观测的噪声的协方差矩阵

在${\hat{x}}_t^{-}$上加上残差就是预测值
K是卡尔曼系数，表示更加相信观测模型还是预测模型，由于观察矩阵和状态矩阵有个能维数不同，这里也是通过K来进行转换

更新最佳估计值的噪声分布，这个是给下一轮迭代用的
在选一轮迭代中由于传递噪声的引入，不确定性又会增大

卡尔曼滤波的完整过程
前面两个公式通过上一时刻的状态预测当前时刻的状态，并不是最佳的估计值
后面的公式通过当前的观测值更新x和P，经过更新后的值就是最佳的观测值

python实现过程

import numpy as np
import matplotlib.pyplot as plt
#观测值
Z=[i for i in range(1,101)]
noise=np.random.normal(size=100)
Z=Z+noise
#状态
X=np.matrix([[0],[0]])
#状态协方差矩阵，在经过迭代后X和P就会趋近于真实状态
P=np.matrix([[1,0],[0,1]])
#状态转移矩阵
F=np.matrix([[1,1],[0,1]])
#状态转移写法差矩阵
Q=np.matrix([[0.00001,0],[0,0.0001]])
#观测矩阵
H=np.matrix([1,0])
#观测噪声方差
R=1

figure=plt.figure()
result=[]
for i in range(100):
    X_=F*X
    P_=F*P*F.T+Q
    K=P_*H.T/(H*P_*H.T+R)
    X=X_+K*(Z[i]-H*X_)
    P=(np.eye(2)-K*H)*P_
    plt.plot(X[0,0],X[1,0],'b.')