卡尔曼过滤 - 实用实施指南（带代码！）

ti

介绍
卡尔曼滤波用于许多应用，包括滤除噪声信号，生成不可观察状态以及预测未来状态。过滤噪声信号至关重要，因为许多传感器的输出噪声也可以直接使用，而卡尔曼滤波可以解释信号/状态的不确定性。生成不可观察状态的一个重要用途是估计速度。通常在不同的关节上安装位置传感器（编码器）; 然而，简单地区分位置以获得速度会产生噪声结果。为了解决这个问题，可以使用卡尔曼滤波来估计速度。卡尔曼滤波器的另一个很好的特性是它可以用来预测未来的状态。当传感器反馈中存在大量时间延迟时，这非常有用，因为这会导致电机控制系统不稳定。

卡尔曼滤波器为线性系统产生最佳估计。因此，传感器或系统必须具有（或接近）线性响应以便应用卡尔曼滤波器。使用非线性系统的技术将在后面的章节中讨论。卡尔曼滤波器的另一个好处是不需要了解长状态历史，因为滤波器仅依赖于直接的最后状态和定义状态正确概率的协方差矩阵。

请记住，协方差只是两个变量如何相互关联的度量（即相互之间的变化）（值通常不直观），协方差矩阵只是告诉您给定的行和列值是什么协方差是。

过滤器
在深入研究过滤器的工作原理之前，讨论术语以确保每个人都有相同的基线是很有用的。状态是指与系统相关的位置/速度/值。操作或输入是您可以更改的将影响系统的项目。一个例子是增加电动机的电压（以增加输出速度）。

（我得到了一个关于我为什么列出位置和速度的问题。简单的答案是，如果你想到一个四轴飞行器，它可以指向一个方向，同时在另一个方向飞行/移动。）

卡尔曼滤波器有两种类型的方程。第一个是预测方程。这些方程式基于先前的状态和命令的动作来预测当前状态。称为更新方程的第二组方程式会查看输入传感器，您对每个传感器的信任程度以及您对整体状态估计的信任程度。该过滤器的工作原理是使用预测方程式预测当前状态，然后通过使用更新方程式检查预测的作业有多好。连续重复该过程以更新当前状态。

预测方程

更新方程式

由于存在许多变量，因此这些方程式看起来很可怕，因此我们现在将阐明每个变量的含义。
x，X =这些变量代表你的状态（即你关心和/或试图过滤的事物）。例如，如果您正在控制具有三个关节的机械臂，您的状态可以是：

这应该在你想要开始的任何状态下初始化。如果您将起始位置视为原点，则可将其初始化为：

虽然我们希望建模并了解系统中的所有状态，但您应该只包含您需要知道的状态。添加更多状态会减慢过滤器的速度并增加整体状态的不确定性。u =对三个关节机械臂的动作是什么，动作可以是：

P，p =这些数字表示过滤器对解决方案的信心程度。执行此操作的最佳方法是在过滤器运行时将其初始化为对角矩阵，它将填充。运行过滤器后，您可以查看值如何收敛，并在下次使用它们初始化过滤器。它应该被初始化为对角线的原因是每个条目直接对应于该行中状态的方差，因此对于具有六个状态的上述机器人臂，协方差矩阵可以初始化为：

其中方差是误差标准差的平方。

Q =过程（即动作）噪声的协方差。它的形成与上面类似，不同之处在于它是您确定的矩阵，并且不会被过滤器更新。该矩阵告诉卡尔曼滤波器从发出命令电压到实际发生的每个动作中有多少误差。
K =该矩阵作为测量更新阶段的一部分进行更新。
H =是传感器的模型，但很难确定。一种简单的方法是将其初始化为对角线单位矩阵并进行调整以改进最终的滤波器结果。
R =与Q类似，除了这定义了每个传感器的置信度。这是进行传感器融合z的关键矩阵
=这些是从每个位置的传感器返回的测量值。卡尔曼滤波器通常用于清除这些信号中的噪声或在没有传感器时估算这些参数。
I =一个标识矩阵（也是对角线）
我们需要确定的下一个变量是A和B.这些矩阵是表示系统如何从一个状态变为另一个状态的模型。
由于卡尔曼滤波器适用于线性系统，我们可以假设如果从头开始（t = 0）并将系统运行到下一个状态（t = 1），并再次运行它（t = 2），那么变化将是相同的，所以我们可以使用该变化，无论我们实际在哪个系统（t =哪里）。

找到A和B矩阵的最简单方法是，如果您能够在使用非强制系统创建过滤器之前通过实验从系统收集数据（输入必须为0）。newState和lastState都是矩阵，其列是实验期间测量的输入和输出状态。为了使其工作，您需要能够测量完整状态，如果您使用卡尔曼滤波器作为状态估计器（这是常见用法），这通常是不可能的。这允许您以数字方式求解A和B. 您需要重复实验n次，其中n是A矩阵的维数。

以来：

然后我们可以将其重写为：

遵循这个逻辑，一旦你有A，你就可以找到B.

了解当前状态，最后状态是什么，A，以及导致状态改变的动作，你可以为B.解决。

注意：在您过滤或融合输入（即传感器）的系统上，您可能无法执行任何操作，因此B *操作术语取消，您需要知道的只是您的A术语。

如果您没有任何先前的数据，那么我们需要一种更正式的方法来确定A和B矩阵。这种方法很难并涉及大量数学。一般的想法是从某个地方开始并扰乱你的系统并记录状态，再次扰乱，再次记录等。

B矩阵可以以相同的方式制作，除了扰乱状态而不是干扰动作并记录状态。
所以在伪代码中，这将是：

X0 =表示你正在线性化;
U0 =产生所需的电压（逆动力学）;

delta =小数;
//找到
ii = 1到#ofStates的矩阵{
X = X0;
X（ⅱ）= X（ⅱ）+增量; //扰动状态x（i）由delta
X1 = f（X，U0）; // f（）是系统
A 的模型（：，ii）=（X1-X0）/ delta;
}

//
为ii = 1到#ofInputs 找到B矩阵{
U = U0;
U（ⅱ）= U（II）+增量; //扰动动作U（i）由delta
X1 = f（X0，U）; // f（）是系统
B 的模型（：，ii）=（X1-X0）/ delta;
}

f（X0，U）是你的模型。该模型可以采取状态和动作，并确定下一个状态。该步骤涉及流动站的运动学和动力学。请记住，如果您的模型没有预测现实生活中发生的事情，那么您的模型是错误的，您需要修复它！

现在我们知道所有变量是什么，我想用单词重复算法。我们从当前状态x k-1，当前协方差矩阵P k-1和当前输入u k-1开始，得到预测状态X和预测协方差矩阵p。然后你就可以得到您的测量Ÿ ķ，并使用更新方程来纠正你的预测中，为了得到你新的状态矩阵X ķ和新的协方差P ķ。完成所有这些操作后，您只需继续迭代执行预测的步骤，然后根据某些已知信息更新预测。

高级过滤方法
对于非线性系统，有两种主要方法。第一个是开发扩展卡尔曼滤波器（EKF）。对于EKF，您需要线性化模型，然后形成A和B矩阵。这种方法涉及一些数学和称为Jacobean的东西，它允许您以不同的方式缩放不同的值。第二种更简单的方法是使用分段近似。为此，您将数据分解为接近线性的区域，并为每个区域形成不同的A和B矩阵。这允许您检查数据并使用过滤器中的相应A和B矩阵来准确预测状态转换将是什么。

示例代码
以下是为PUMA 3DOF机器人手臂设计的卡尔曼滤波器的c ++代码。此代码用于速度估计，因为这比仅区分位置更准确。
我对噪声和传感器模型做了很好的假设，以简化实现。我还将我的协方差初始化为单位矩阵。在我的真实代码中，我让它收敛并将其保存到我每次启动过滤器时都能读取的文本文件中。

很久以前我做过这段代码。我现在对代码看起来有点尴尬，但我仍然会分享它。

/ ******************* ******
*卡尔曼过滤器PUMA 3DOF机械臂
********************************* ******* /
#include
#include
#include
#include
#include
#include
//我的矩阵库，你可以使用自己喜欢的矩阵库
#include“matrixClass.h”
#define tiMESTEP 0.05

使用命名空间std;

float timeIndex，m_a1，m_a2，m_a3，c_a1，c_a2，c_a3，tau1，tau2，tau3;
float a1，a2，a3，a1d，a2d，a3d，a1dd，a2dd，a3dd;
float new_a1d，new_a2d，new_a3d;
float TIME = 0;
矩阵A（6,6）;
矩阵B（6,3）;
矩阵C =矩阵::眼睛（6）; //将它们初始化为6×6单位矩阵
矩阵Q = matrix :: eye（6）;
矩阵R =矩阵::眼睛（6）;
矩阵y（6,1）;
矩阵K（6,6）;
矩阵x（6,1）;
矩阵状态（6,1）;
矩阵行动（3,1）;
matrix lastState（6,1）;
matrix P = matrix :: eye（6）;
matrix p = matrix :: eye（6）;
矩阵测量（6,1）;

void initKalman（）{

浮起[6] [6] = {
{1.004，0.0001,0.001，0.0014，0.0000，-0.0003}，
{0.000，1.000，-0.00，0.0000，0.0019，0}，
{0.0004，0.0002，1.002，0.0003，0.0001 ，0.0015}，
{0.2028,0.0481,0.0433,0.7114，-0.0166，-0.1458}，
{0.0080,0.0021，-0.0020，-0.0224,0.9289,0.005}，
{0.1895,0.1009,0.1101，-0.1602,0.0621,0.7404}
};

float b [6] [3] = {
{
0.0000,0.0000,0.0000}，
{0.0000,0.0000，-0.0000}，{0.0000，-0.0000,0.0000}，
{0.007，-0.0000,0.0005}，
{0.0001,0.0000， -0.0000}，
{0.0003，-0.0000,0.0008}
};

/ *从上面加载A和B矩阵* /
for（int i = 0; i <6; i ++）{
for（int j = 0; j <6; j ++）{
A [j] = a [ i] [j];
}
}
对于（INT I = 0; I <6;我++）{
对于（INT J = 0;Ĵ<3; J ++）{
B [j] = B [j];
}
}

/ *初始化状态* /
state [0] [0] = 0.1;
状态[1] [0] = 0.1;
状态[2] [0] = 0.1;
状态[3] [0] = 0.1;
状态[4] [0] = 0.1;
状态[5] [0] = 0.1;

lastState =状态;

}

void kalman（）{
lastState = state;
状态[0] [0] = C_A1;
状态[1] [0] = C_A2;
状态[2] [0] = c_a3;
状态[3] [0] = A1D;
状态[4] [0] = A2D;
状态[5] [0] = A3D;

测量[0] [0] = m_a1;
测定[1] [0] = m_a2;
测量[2] [0] = m_a3;
测量[3] [0] = A1D;
测量[4] [0] = A2D;
测量[5] [0] = A3D;

操作[0] [0] = TAU1;
操作[1] [0] = tau2;
动作[2] [0] = tau3;

矩阵temp1（6,6）;
矩阵temp2（6,6）;
矩阵temp3（6,6）;
矩阵temp4（6,1）;
/ ************预测方程***************** /
x = A * lastState + B * action;
p = A * P * A'+ Q;
/ ************更新公式********** /
K = p * C * pinv（C * p * C'+ R）;

Y = C *状态;

state = x + K *（yC * lastState）;

P =（眼睛（6） - K * C）* p;

A1 =状态[0] [0];
A2 =状态[1] [0];
A3 =状态[2] [0];
A1D =状态[3] [0];
A2D =状态[4] [0];
A3D =状态[5] [0];
}

/ *此函数未使用，因为我使用position来获得速度（即区分）。
*但是我认为如果你想要
从加速度中获得速度和位置*你会使用它是有用的* /
void integrate（）{
new_a1d = a1d + a1dd * TIMESTEP;
a1 + =（new_a1d + a1d）* TIMESTEP / 2;
a1d = new_a1d;
new_a2d = a2d + a2dd * TIMESTEP;
a2 + =（new_a2d + a2d）* TIMESTEP / 2;
a2d = new_a2d;
new_a3d = a3d + a3dd * TIMESTEP;
a3 + =（new_a3d + a3d）* TIMESTEP / 2;
a3d = new_a3d;
TIME + = TIMESTEP;
}

/ *这给了我来自位置的速度* /
void differentiation（）{
a1d =（state [0] [0] -lastState [0] [0]）/ TIMESTEP;
A2D =（状态[1] [0] -lastState [1] [0]）/ TIMESTEP;
A3D =（状态[2] [0] -lastState [2] [0]）/ TIMESTEP;
TIME + = TIMESTEP;
}

int main（）{
initKalman（）;
char buffer [500];
ifstream readFile（“DATA.txt”）; //这是我读取数据的地方，因为我全部离线处理它

while（！readFile.eof（））{
readFile.getline（buffer，500）;
sscanf（缓冲区，“％f％f％f％f％f％f％f％f％f％f”，＆timeIndex，＆m_a1，＆m_a2，＆m_a3，＆c_a1，＆c_a2，＆c_a3，＆tau1，＆tau2，＆tau3）;

卡尔曼（）;
分化（）;
//整合（）;

/ *这是我将结果记录到/和/或将其打印到屏幕的地方* /
FILE * file = fopen（“filterOutput.txt”，“a”）;
fprintf（文件，“％f％f％f％f％f％f％f％f％f％f n”，TIME，a1，a2，a3，a1d，a2d，a3d，tau1，tau2，tau3）;
fprintf（stderr，“％f％f％f％f％f％f％f％f％f％f n”，TIME，a1，a2，a3，a1d，a2d，a3d，tau1，tau2，tau3）;
FCLOSE（文件）;
}
return 1;
}