了解卡尔曼滤波

weixin_46834504

已于 2023-03-20 13:38:53 修改

阅读量211

点赞数

文章标签：矩阵线性代数

于 2023-03-17 20:00:00 首次发布

本文链接：https://blog.csdn.net/weixin_46834504/article/details/129492111

版权

本文介绍了卡尔曼滤波的基本概念和数学推导，包括状态方程和更新方程。通过示例展示了卡尔曼滤波在电动车BMS和自动驾驶中的应用。文中还提供了两个Python代码实现，分别模拟了车辆位置估计和温度测量的滤波过程，以加深理解。

摘要由CSDN通过智能技术生成

我们会经常看到或者听说卡尔曼滤波。在电动车的BMS(Battery manangemen system)里面，也会用到卡尔曼滤波，当然更多的可能是会用到AKF 自适应卡尔曼滤波。在自动驾驶中，也会用到自适应卡尔曼滤波。自适应卡尔曼滤波主要是使用历史固定帧的数据去更新R和Q矩阵，只更新R矩阵的做法比较多，并且只用了加权求和的方法，动态调节参数没有使用。

本文中以下的内容主要是了解卡尔曼滤波，没有涉及AKF 自适应卡尔曼滤波。我们可以在网上找到很多关于卡尔曼滤波的内容，但是可能自己把这些内容写一遍，能够做到对卡尔曼滤波的更深入一些的了解。

状态方程：

预测（时间更新）：

更新（测量更新）：

, 需要人为设置初始值，，F，B和H为已知量，和也认为是已知量。得出预测值，，有了，P0- 可以得到更新值，进而得到，。那么时刻的计算就完成了。

, 根据已经得到的，，得出预测值，，有了，可以得到更新值，进而得到，。那么时刻的计算就完成了。

以此继续，

推导过程 :

把真实值和预测值的差叫做先验误差：

把真实值和估计值的差叫做后验误差：

那么问题就转化为求解，使其最小化。可以通过MSE(mean square error),使得后验误差最小。

估计值 = 预测值 + 权重 x 观测误差，也就是说公式，是直接就定义好的，不需要再次推导了。公式是输出的状态方程，也不需要再推导。

把公式带入公式 :

得出：

分别定义先验误差和后验误差的协方差矩阵为- ，

在卡尔曼滤波中，都假定造成服从高斯分布。写成是说明每行是一个特征，从而求得的协方差矩阵。

把公式带入到公式中，并且由于et- 和vt 不相关，Eet-vtT=E(vtet-T ，得到：

展开：

因为：，并且是自相关矩阵，，所以：

根据MSE(mean square error)，使得后验误差最小。可以看到，对角线上的值就是各个元素的方差。对角线上的和又称为矩阵的迹(Trace)，用来做为损失函数，记作 . 根据迹的性质，一个方阵的迹和它的转置的迹相等，因此：

把上式看成的凸函数，导数为零的点就是最小值。求导：

得到：

根据公式，得出：

所以公式) 演变为

进而

即：

计算协方差，再利用和不相关的假设：

)

即：

例1：假如有一辆无人驾驶汽车，里面装了gps来测量小车的位置，装了车速表来测量小车的车速，它沿着一条直路行驶，已知它在时刻的位置是 , 速度是 , 加速度为 , 那么它在时刻的和速度是多少呢？

其中是和时刻的时间差。

写成状态方程的形式 :

定义小车的状态：

定义状态转移矩阵：

定义控制矩阵：

加入过程噪声，公式变成：

其中 .

定义gps测量的位置信息为，

其中，，为测量噪声

虽然和都是实际上未知的，但是为了简化问题，我们假设过程噪声和测量噪声都是高斯白噪声，并假设他们是已知的。并他们的协方差矩阵分别为和 , 即，记作：。

import numpy as np
import pandas as pd
import matplotlib.pyplot as plt
deltaT=1
time=range(0,100,deltaT) #时间序列，单位是s
vhlSpd=[5 for i in range(len(time))] #小车的初始速度为5m/s
gpsPos=[0 for i in range(len(time))] # 小车的初始位置为0

acc = 0.1 # 小车的加速度为0.1m/s^2
for i in range(1,len(time)):
    vhlSpd[i]=vhlSpd[i-1]+acc*deltaT
    gpsPos[i]=gpsPos[i-1]+vhlSpd[i-1]+0.5*acc*deltaT*deltaT

noise_vhlSpd = 0.01*np.random.randn(len(time)) #人为制造一些噪声数据
noise_gpsPos = 3*np.random.randn(len(time)) #人为制造一些噪声数据
vhlspd = vhlSpd+noise_vhlSpd     # 人为制造的， 车速表的测量数据， 相当于Zt
gpsPos = gpsPos + noise_gpsPos   # 认为制造的，gps的测量数据, 相当于Zt

#设置卡尔曼当中所需的已知量
Q = np.mat([[0.01, 0],[0, 0.01]]) # 过程噪声的协方差矩阵
R = np.mat([1])

#初始化
X = np.mat([[0],[0]]); #初始状态，[位置，速度]就是我们要估计的内容
P = np.mat([[1, 0],[0, 1]]) #先验误差协方差矩阵的初始值，根据经验给出

#已知的线性变换矩阵
F = np.mat([[1,deltaT],[0,1]])
B = np.mat([[0.5*deltaT*deltaT], [deltaT]])
H = np.mat([1,0])

#对每一时刻状态的记录
xlist=[]
for i in range(0, len(time)):
    X_ = F*X + B*acc
    P_ = F*P*F.T + Q
    K = P_*H.T/(H*P_*H.T + R)
    X = X_+K*(np.mat(gpsPos[i])-H*X_)
    P = (np.eye(2)-K*H)*P_
    xTurn = X[0].tolist()
    xTurn1 = xTurn[0]
    xlist.append(xTurn1[0])

fig, ax = plt.subplots(figsize=[10,5],facecolor='lightblue')

ax.set_facecolor('#eafff5')

ax.set_title('Estimated position vs Measured position', color='darkblue')
# 4) single letter color string
ax.set_xlabel('Time [s]',  color='darkblue',fontsize=12)
# 5) a named color:
ax.set_ylabel('gps postion', color='darkblue',fontsize=12)
# 6) a named xkcd color:
ax.plot(time, xlist, 'xkcd:crimson',label= 'Estimated gps postion')
# 7) Cn notation:
ax.plot(time, gpsPos, color='black', linestyle='--', label= 'Measured gps postion')
# 8) tab notation:
ax.tick_params(labelcolor='darkblue',color='darkblue',width=2)
ax.spines['left'].set_color('darkblue')
ax.spines['bottom'].set_color('darkblue')
ax.spines['right'].set_color('none')
ax.spines['top'].set_color('none')
ax.grid(axis='y')

labelss = ax.legend(loc=0, fontsize=12).get_texts()
[label.set_color('darkblue') for label in labelss]

例2：假设如下一个系统，房间内连续两个时刻温度差值的标准差为0.02度，温度计的测量值误差的标准差为0.5度，房间温度的真实值为24度，对温度的初始估计值为23.5度，误差的方差为1。

Python 代码实现:

# 假设如下一个系统：
# 房间内连续两个时刻温度差值的标准差为0.02度
# 温度计的测量值误差的标准差为0.5度
# 房间温度的真实值为24度
# 对温度的初始估计值为23.5度，误差的方差为1
# Kalman filter example of temperature measurement in Matlab, I use python to do it
import numpy as np
import pandas as pd
import matplotlib.pyplot as plt

n_iter=range(0,100,1) # 0,...99s，100 个时刻
z = 24+np.random.randn(len(n_iter)) # z是温度计的测量结果，在真实值（永远未知）24的基础上加上了方差为0.25的高斯噪声。

#设置卡尔曼当中所需的已知量
Q = np.mat([4e-4]) # 过程噪声的协方差矩阵
R = np.mat([0.25])

#初始化
X = np.mat([23.5]); #初始状态，
P = np.mat([1]) #先验误差协方差矩阵的初始值，根据经验给出
u=0
#已知的线性变换矩阵
F = np.mat([1])
B = np.mat([0])
H = np.mat([1])

#对每一时刻状态的记录
xlist=[]
for i in range(0, len(n_iter)):
    X_ = F*X + B*u
    P_ = F*P*F.T + Q
    K = P_*H.T/(H*P_*H.T + R)
    X = X_+K*(np.mat(z[i])-H*X_)
    P = (np.eye(1)-K*H)*P_
    xTurn = X[0].tolist()
    xTurn1 = xTurn[0]
    xlist.append(xTurn1[0])

fig, ax = plt.subplots(figsize=[10,5],facecolor='lightblue')

ax.set_facecolor('#eafff5')

ax.set_title('Estimated temperature vs Measured temperature', color='darkblue')
# 4) single letter color string
ax.set_xlabel('Time [s]',  color='darkblue',fontsize=12)
# 5) a named color:
ax.set_ylabel('temperature', color='darkblue',fontsize=12)
# 6) a named xkcd color:
ax.plot(n_iter, xlist, 'xkcd:crimson',label= 'Estimated temperature')
# 7) Cn notation:
ax.plot(n_iter, z, color='black', linestyle='--', label= 'Measured temperature')
# 8) tab notation:
ax.tick_params(labelcolor='darkblue',color='darkblue',width=2)
ax.spines['left'].set_color('darkblue')
ax.spines['bottom'].set_color('darkblue')
ax.spines['right'].set_color('none')
ax.spines['top'].set_color('none')
ax.grid(axis='y')

labelss = ax.legend(loc=0, fontsize=12).get_texts()
[label.set_color('darkblue') for label in labelss]