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

预测方程

更新方程式

以来：

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

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

了解当前状态，最后状态是什么，A，以及导致状态改变的动作，你可以为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;
}

/ ******************* ******
*卡尔曼过滤器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;
}