跳至主要內容

卡尔曼滤波器:递归算法

丁毅桂2025年1月5日大约 2 分钟

卡尔曼滤波器学习笔记

【卡尔曼滤波器】_DR_CAN合集学习笔记

卡尔曼滤波器(Kalman Filter)

卡尔曼滤波器广泛应用的原因：世界中存在大量不确定性

递归算法

案例：

测量结果

第n次测量结果ZnZ_n
Z1=50.1mmZ_1=50.1mm
Z2=50.4mmZ_2=50.4mm
Z2=50.2mmZ_2=50.2mm

求平均数公式推导递归算法：

X^n=1n(Z1+Z2+⋯+Zn)=1n(Z1+Z2+⋯+Zn−1)+1nZn=n−1n1n−1(Z1+Z2+⋯+Zn−1)+1nZn=n−1nX^n−1+1nZn=X^n−1−1nX^n−1+1nZn=X^n−1−1n(Zn−X^n−1) \begin{align*} \hat{X}_n &= \frac{1}{n}(Z_1+Z_2+\dots+Z_n) \\ &= \frac{1}{n}(Z_1+Z_2+\dots+Z_{n-1})+\frac{1}{n}Z_n \\ &= \frac{n-1}{n}\frac{1}{n-1}(Z_1+Z_2+\dots+Z_{n-1})+\frac{1}{n}Z_n \\ &= \frac{n-1}{n}\hat{X}_{n-1}+\frac{1}{n}Z_n \\ &= \hat{X}_{n-1}-\frac{1}{n}\hat{X}_{n-1}+\frac{1}{n}Z_n \\ &= \hat{X}_{n-1}-\frac{1}{n}(Z_n-\hat{X}_{n-1}) \\ \end{align*}

结论

X^n=X^n−1−1n(Zn−X^n−1)本次估计=上次估计+1n(本次测量−上次估计) \hat{X}_n = \hat{X}_{n-1}-\frac{1}{n}(Z_n-\hat{X}_{n-1}) \\ \text{本次估计} = \text{上次估计} + \frac{1}{n}(\text{本次测量}-\text{上次估计}) \\

n→+∞;1n→0;X^n→X^n−1;随着测量次数增加；测量结果不再重要；估计值逐渐稳定；n→0;1n→+∞;X^n→Zn;测量次数较小时；测量结果作用较大；估计值受测量结果影响较大； n\to+\infty; \frac{1}{n}\to 0; \hat{X}_n \to \hat{X}_{n-1}; \\ \text{随着测量次数增加；测量结果不再重要；估计值逐渐稳定；}\\ n\to0; \frac{1}{n}\to +\infty; \hat{X}_n \to Z_n; \\ \text{测量次数较小时；测量结果作用较大；估计值受测量结果影响较大；}\\

卡尔曼增益K

把上面案例中的系数1n\frac{1}{n}换成代表卡尔曼增益的系数K,

那么就得到了卡尔曼滤波算法中的第一个公式：

X^n=X^n−1+Kn(Zn−X^n−1)本次估计=上次估计+增益系数K(本次测量−上次估计) \hat{X}_n = \hat{X}_{n-1}+K_n(Z_n-\hat{X}_{n-1}) \\ \text{本次估计} = \text{上次估计} + \text{增益系数K}(\text{本次测量}-\text{上次估计})

其中卡尔曼增益K

Kn=eESTn−1eESTn−1+eMEAn本次卡尔纳曼增益=上次估计误差上次估计误差+本次测量误差 K_n=\frac{e_{EST_{n-1}}}{e_{EST_{n-1}}+e_{MEA_{n}}} \\ \text{本次卡尔纳曼增益} = \frac{\text{上次估计误差}}{\text{上次估计误差+本次测量误差}}

分析：

卡尔曼滤波计算三步骤

第一步:计算卡尔曼增益

Kn=eESTn−1eESTn−1+eMEAn K_n=\frac{e_{EST_{n-1}}}{e_{EST_{n-1}}+e_{MEA_{n}}}

第二步：计算估计值

X^n=X^n−1+Kn(Zn−X^n−1) \hat{X}_n = \hat{X}_{n-1}+K_n(Z_n-\hat{X}_{n-1}) \\

第三步：更新估计误差

eESTn=(1−Kn)eESTn−1 e_{EST_{n}}=(1-K_n)e_{EST_{n-1}}

案例

import numpy as np
import matplotlib.pyplot as plt

# 数据

N = 50
Z = []
X = [0] # 估计值初值0

Error_mes = 5 # 测量误差
Error_est = 50 # 估计误差 初值50mm
K = 0 # 卡尔曼增益

# 生成随机数据： 均值 50mm 测量误差 5mm
for i in range(0,N):
    Z.append(np.random.normal(50,5)) 

for i in range(1,N):
    K = Error_est/(Error_est+Error_mes) # 卡尔曼增益=估计误差/（估计误差+测量误差）
    x = X[i-1] + K * (Z[i] - X[i-1]) # 估计值=测量值-K*(测量值-真实值)
    Error_est = (1-K) * Error_est # 估计误差=（1-K）*估计误差
    X.append(x)


# 绘制图形
plt.scatter(range(0,len(Z)),Z) # 点图 测量值
plt.plot(range(0,len(X)),X)  # 折线图 估计值
# 显示图形
plt.show()

alt text