卡尔曼滤波程序_卡尔曼滤波的初步理解

这篇博客写的很好，主要的理解过程是从他这里得到的，写的都是矩阵运算，慢慢看挺好懂的，重点是里面对高斯分布的乘积的理解就是对卡尔曼滤波来源的理解：

轻松理解卡尔曼滤波

这篇文章写了一个比较简单的python代码，从代码这里又获得了进一步的理解：

https://blog.csdn.net/codesamer/article/details/81191487

---

以下是我对卡尔曼滤波理解过程中做的一些思考和笔记：

---

涉及到的各个参数及其含义：

xt t 时刻的状态，预测值，估计值

zt t 时刻的观察值

Q，R Q 预测模型噪音的协方差矩阵，R 观测噪音的协方差矩阵；

H 当前状态和当前观测值的关系矩阵，用它可以从当前状态推导出当前观测值，但是这是推导出的，不是实际观测到的吗？？？ 乘以 H 后才最终完成了预测值

K 卡尔曼增益 权重！

P 预测值的距离和速度之间的误差协方差矩阵？ 表示预测的不确定性； 噪声协方差矩阵对系统会产生影响，所以我们要传递到系统中去

Fk 状态转移矩阵 （比如速度，距离转移矩阵）

Bk,Uk 状态控制矩阵 , 状态控制向量 (比如加速度状态转移矩阵，加速度) ； 一般没有用上

---

不断迭代的系统变量只有 3 个，分别是系统的状态 Xt ，其误差协方差矩阵 P ，和卡尔曼增益 K ;

---

Q，R 和 P的区别是什么？

Q 是预测模型的误差，是状态转移的过程中加入的，比如用匀速模型或者匀加速模型去预测物体状态造成的误差；

R 是观测测量的误差，这是传感器的特性决定的，很好理解；

P 是上一个时刻估计出来状态的误差；从它的更新公式可以看出来，它是由最初的 P 和 R 一起迭代生成的，所以它实际上就是上一个时刻的综合的误差。

误差分为两大块，预测结果和观测结果的误差，P 和 Q 合起来才是最终预测值的误差， R 单独就是观测误差；

---

H 的作用是什么？

对观测值做一个形式的变化，不改变实质；

由于我们的状态变量是一个二维矩阵，而表示距离的是一个标量，所以状态变量矩阵就要乘以一个观测矩阵H，当然我们观测到的位置也不一定是绝对准确的，所以也要加上一个观测噪声

---

核心的思想是什么？

1 用加权平均的观测值和预测值表示真实值（真实值始终是未知的，只能接近）

2 K 的来源是两个误差高斯概率密度函数的乘积（最后公式使用预测结果和观测结果的方差，也就是 P 和 R )； 大概表达成公式就是 K=P/(P+R)

3 理解多个误差分布才是理解卡尔曼滤波的核心！

---

权重的调整怎么进行的？

K 的核心思想表达成大概的公式是 K=P/(P+R) ,那么如果上一时刻的 P 过大，这次得到的权重就会比较大，从而在加权计算中更加偏向于观测值，反之亦然；

K=P/(P+R) 怎么来的看第一个博客链接；

---

不存在真值，也没有和真值相减的过程，怎么知道 P 的大小是否合理呢？

待解答。

---

我对第二个链接代码做的进一步注释：

# coding:utf-8
# 卡尔曼滤波理解代码
# 模拟预测小车位置和速度的一段程序，
# 观测的值是一段从0到99，速度为1的匀速行驶路径，
# 加入的噪声是均值为0，方差为1的高斯噪声


import numpy as np
import matplotlib.pyplot as plt



# x 初始状态,表示离原点距离和速度
# 为什么这里有2个值，zmat 又每次只有1个值？
x_mat = np.mat([[0,],[0]])


# 观测值Z
z = np.arange(100)
# 值得注意的是它每次只有一个值，表示距离，只观测当前距离
z_watch = np.mat(z)


# 观测噪音 R ,是一个 N（0,1）分布的随机数
R_noise = np.random.normal(0,1,100)
R_noise_mat = np.mat(R_noise)

# 观测值Z + 观测噪音R
z_mat = z_watch + R_noise_mat



# 初始状态协方差矩阵 P，开始是全然不相关的；记住它是综合误差
p_mat = np.mat([[1,0],[0,1]])

# 状态转移矩阵F，
f_mat = np.mat([[1,1],[0,1]])

# 状态转移协方差矩阵 Q, 协方差比较小，
# 说明状态转移矩阵比较正确，预测模型比较准确；
q_mat = np.mat([[0.0001,0],[0,0.0001]])

# 观测转换矩阵 H，shape是1*2；把它和x_predict（2*1）相乘，才能转换得到 1 个的距离值
h_mat = np.mat([1,0]) # 1，0 表示只取第一个

# 观测噪音协方差 R ,为1 表示没有噪音？不是，表示观测误差方差恒定为1；
# 因为我们观测值的分布就是 （0,1）分布；
r_mat = np.mat([1])


for i in range(100):
    x_predict = f_mat * x_mat
    print('x_predict',x_predict)
    p_predict = f_mat * p_mat * f_mat.T + q_mat
    print('p_predict',p_predict)  # 预测值最终误差，左下角和右下角元素从大变小，因为开始速度准确性比较低，后面比较准确了

    kalman = p_predict * h_mat.T / (h_mat * p_predict * h_mat.T + r_mat)
    print('kalman',kalman)
    # kalman 的第二个值也是一开始比较大，说明比较偏向于速度观测值，后续变小
    x_mat = x_predict + kalman*(z_mat[0,i] - h_mat * x_predict)
    print('z_mat[0,i] - h_mat * x_predict',z_mat[0,i] - h_mat * x_predict) # 这一步只得到了一个值的结果
    print('kalman*(z_mat[0,i] - h_mat * x_predict)',kalman*(z_mat[0,i] - h_mat * x_predict))
    p_mat = (np.eye(2) - kalman * h_mat) * p_predict
    print('p_mat',p_mat)
    # p_mat 的协方差的左下角和右下角元素从大变小，因为开始速度准确性比较低，后面比较准确了

    plt.plot(x_mat[0,0], x_mat[1,0], 'ro', markersize=1)

# 可以看到，刚开始，速度值x_mat[1,0]初始值是0，和实际相差较大，但后续越来越准确
plt.show()