【发布时间】：2015-03-27 13:13:21
【问题描述】：
我正在研究 stm32f417ve 臂处理器并尝试实现卡尔曼滤波器，以将加速度计数据与高度（压力传感器）数据融合。 我想知道估计的垂直速度和位置。加速度计读数从车身框架旋转到地球框架，这不是问题。 我已经在互联网上搜索了很多，也发现了一些有趣的东西，但我不确定我的情况是否适合我找到的其他情况，所以我在这里:) 这篇文章 (Using Kalman filter with acceleration and position inputs) 与这篇文章非常相似，但我需要更多帮助。 我还有一个 MPU6000 作为 6DOF imu 和一个 MS5611 baro。我认为，结合这些数据的最佳方式是使用加速度作为控制输入，对吗？ 也许有人可以看看我的矩阵和公式来告诉我，它是否正确。
公式：
//PREDICT
x = A*x + B*u
p = A*p*AT + Q
//UPDATE
Innovation = (H*p*HT + R)^-1
K = p*HT*Innovation
x = x + K*(y-H*x)
p = (I-K*H)*p
矩阵化：
#define NumState 3
#define NumInput 1
#define NumOutput 1
static float32_t xAr[NumState][1];
static float32_t uAr[NumInput][1];
static float32_t yAr[NumOutput][1];
static float32_t AAr[NumState][NumState];
static float32_t BAr[NumState][NumInput];
static float32_t HAr[NumOutput][NumState];
static float32_t QAr[NumState][NumState];
static float32_t RAr[NumOutput][NumOutput];
static float32_t PAr[NumState][NumState];
static float32_t kAr[NumState][NumOutput];
static float32_t IAr[NumState][NumState];
我将加速度放入向量 u 中，将高度放入 y 中。 Matrix IAr 只是一个单位矩阵，所以它的对角元素是 1。
RAr[0][0] = 0.1f;
QAr[0][0] = 1.0f;
QAr[0][1] = 1.0f;
QAr[0][2] = 0.0f;
QAr[1][0] = 1.0f;
QAr[1][1] = 1.0f;
QAr[1][2] = 0.0f;
QAr[2][0] = 0.0f;
QAr[2][1] = 0.0f;
QAr[2][2] = 0.0f;
uAr[0][0] = AccZEarth;
yAr[0][0] = Height;
HAr[0][0] = 1.0f;
HAr[0][1] = 0.0f;
HAr[0][2] = 0.0f;
BAr[0][0] = (dt*dt)/2;
BAr[1][0] = dt;
BAr[2][0] = 0.0f;
AAr[0][0] = 1.0f;
AAr[0][1] = dt;
AAr[0][2] = 0.0f - ((dt*dt)/2.0f);
AAr[1][0] = 0.0f;
AAr[1][1] = 1.0f;
AAr[1][2] = 0.0f - dt;
AAr[2][0] = 0.0f;
AAr[2][1] = 0.0f;
AAr[2][2] = 1.0f;
IAr[0][0] = 1.0f;
IAr[0][1] = 0.0f;
IAr[0][2] = 0.0f;
IAr[1][0] = 0.0f;
IAr[1][1] = 1.0f;
IAr[1][2] = 0.0f;
IAr[2][0] = 0.0f;
IAr[2][1] = 0.0f;
IAr[2][2] = 1.0f;
PAr[0][0] = 100.0f;
PAr[0][1] = 0.0f;
PAr[0][2] = 0.0f;
PAr[1][0] = 0.0f;
PAr[1][1] = 100.0f;
PAr[1][2] = 0.0f;
PAr[2][0] = 0.0f;
PAr[2][1] = 0.0f;
PAr[2][2] = 100.0f;
如果你们中的一些人可以看看并告诉我我是对还是错，那就太好了！
谢谢， 克里斯
【问题讨论】：
标签： height stm32 kalman-filter