Python卡尔曼数字滤波(线性二次估计)一维和多维传感器测量

卡尔曼数字滤波

对于统计和控制理论，卡尔曼滤波，也称为线性二次估计 (LQE)，是一种算法，它使用随时间观察到的一系列测量值，包括统计噪声和其他不准确性，并产生对未知变量的估计，这些估计往往通过估计每个时间范围内变量的联合概率分布，比仅基于单个测量的结果更准确。

应用领域

卡尔曼滤波有许多技术应用。 一个常见的应用是引导、导航和控制车辆，特别是动态定位的飞机、航天器和船舶。此外，卡尔曼滤波是一个广泛应用于时间序列分析的概念，用于信号处理和计量经济学等主题。 卡尔曼滤波也是机器人运动规划和控制的主要课题之一，可用于轨迹优化。 卡尔曼滤波也适用于模拟中枢神经系统对运动的控制。 由于发出电机命令和接收感官反馈之间的时间延迟，卡尔曼滤波的使用为估计电机系统的当前状态和发出更新的命令提供了一个现实的模型。

算法特点

该算法通过两阶段过程工作。 对于预测阶段，卡尔曼滤波产生当前状态变量的估计，以及它们的不确定性。 一旦观察到下一次测量的结果（必然会因一些错误，包括随机噪声而损坏），这些估计值就会使用加权平均值进行更新，对具有更大确定性的估计值赋予更大的权重，该算法是递归的。 它可以实时运行，仅使用当前输入测量值和先前计算的状态及其不确定性矩阵，不需要额外的过去信息。

卡尔曼滤波传感器融合

一维测量

卡尔曼滤波器是离散系统，它允许我们通过自变量定义因变量，我们将在其中求解自变量，以便在给定测量值（因变量）时，我们可以推断自变量的估计，假设噪声存在于我们的输入测量中，并且噪声也存在于我们用数学方程对世界建模的方式中，因为在非无菌世界中不可避免地存在无法解释的因素。输入变量在建模为方程组时变得更有价值，或者矩阵，以便确定这些值之间的关系。 每个维度中的每个变量都将包含噪声，因此相关输入的引入将允许基于下一步的预测差分、系统中未考虑的噪声以及传感器输入引入的噪声进行加权平均。

import numpy as np
import pandas as pd
import matplotlib.pyplot as plt
import matplotlib.mlab as mlab
import seaborn as sb
from scipy import stats
import time

绘制此范围的分布图

plt.figure(figsize=(fw,5))
plt.plot(x,mlab.normpdf(x, mean0, var0), label='Normal Distribution')
plt.ylim(0, 0.1);
plt.legend(loc='best');
plt.xlabel('Position');

预测移动距离

平均值是米，由速度*dt 或计步器或车轮编码器计算得出。VarMove 是通过静态测量估计或确定的

plt.figure(figsize=(fw,5))
plt.plot(x,mlab.normpdf(x, meanMove, varMove), label='Normal Distribution')
plt.ylim(0, 0.1);
plt.legend(loc='best');
plt.xlabel('Distance moved');

多维测量

状态矢量

Matplotlib绘制协方差矩阵

测量噪声协方差

算法预测步骤

源代码

详情参阅 - 亚图跨际