Question

我正在研究一个stm32f417ve臂式处理器并试图实现一个卡尔曼滤波器，用于将加速度计数据与高度（压力传感器）数据融合。我想知道估计的垂直速度和位置。加速度计读数从车身框架旋转到地球框架，这不是问题。我已经在互联网上搜索过很多东西并且发现了一些有趣的东西，但是我不确定我的情况是否适合我发现的其他情况，所以我在这里：）这篇文章（Using Kalman filter with acceleration and position inputs）与这篇文章非常相似，但我需要更多的帮助。我还有一台MPD6000作为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

Matrizes：

#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;

如果你们中的一些人可以看一看并告诉我我是对还是错，那真的很棒！

谢谢，克里斯

Answer 1

首先要确定的是，您打算一起使用的两个传感器是否是一个很好的补充。随着双重积分误差的增加，MEMS IMU位置将迅速发散。要在此应用程序中成功使用它，您必须校准其偏差和比例。这些在每个轴上都是不同的，给定您的一维状态，必须在滤波器外部应用。由于您可能会在户外（高度计很有趣），您的偏差/刻度校准也应该进行温度补偿。

当IMU放在桌面上时，您可以通过x = A*x + B*u循环轻松测试IMU，以查看x[0]变大的速度。鉴于我对IMU和高度计（不像IMU）的了解，我猜你的IMU派生位置会比几秒钟内的原始高度计读数更差。如果偏差和比例没有得到正确校准，速度会快得多。卡尔曼滤波器只值得＆＃34;融合＆＃34;这两个传感器，如果你能达到IMU的短期精度明显优于高度计的短期精度的点。

如果您继续使用KF，您的结构通常看起来很好。以下是一些具体意见：

您将加速模型设为-x[2]。（减号是由于你的矩阵A。我不确定你为什么选择否定加速度。）我不认为你所在州的加速对你有好处。 ... + B*u使用IMU方法的一个优点是您不必保留加速度（正如您的B矩阵所示）。如果加速是测量，则必须将其放在状态向量中，因为H=[0 0 1]。
您尚未付出任何努力选择P，Q或R。这些是KF中最重要的矩阵。这里的另一个答案可能有所帮助：https://electronics.stackexchange.com/questions/86102/kalman-filter-on-linear-acceleration-for-distance/134975#134975

Answer 2

谢谢你的回答！到目前为止，我正在使用互补滤波器将acc数据与baro数据融合。 acc的所有三个轴都已经过补偿。现在，我有一个1D-kalman滤波器，它可以降低baro输出的噪声，同时保持相位延迟非常小，这就是我不使用低通滤波器的原因。我正在计算baro数据的导数以根据baro得到速度，baro延迟约100ms。然后通过对该速度进行积分将该速度与acc计算的速度一起馈送到第一互补滤波器中。第二个互补滤波器使用这个融合速度（它是无漂移的并且几乎没有延迟）并将其集成以将其与巴罗高度数据融合。这非常有效，但我想尝试使用卡尔曼滤波器来查看，从中可以获得更准确的数据。在互联网上，如果找到这篇论文：http://www.actawm.pb.edu.pl/volume/vol8no2/06_2014_004_ROMANIUK_GOSIEWSKI.pdf 它似乎很符合我的“问题”，所以我决定用它作为起点。我的矩阵A中的负号来自于此，可能是由于它们的安装方向。我要检查一下;）

最诚挚的问候克里斯

卡尔曼滤波器用于高度和加速度

2 个答案: