卡尔曼滤波 IMU 噪声 c#

Question

我有以下问题。我想消除 IMU 传感器的噪声。我的线索是卡尔曼滤波器。在 Arduino IDE 中，您可以通过库轻松实现一个。现在我想直接在计算机上通过 C# 解决这个问题，但我在 .NET 4 上找不到可用的库。我用 NugetPackages 试过了：MathNet。和 Emgu.CV。您是否有适用于 .NET 4.0 的替代方案，或者它们是否有效？如果可行，有没有人有一个很好的例子？祝你有美好的一天:)

编辑：添加 Arduino IDE 代码

#include <Adafruit_MPU6050.h>
#include <Adafruit_Sensor.h>
#include <Kalman.h>
#include <Wire.h>


// Dimensions of the matrices
#define Nstate 2 // length of the state vector
#define Nobs 2   // length of the measurement vector
// measurement std (to be characterized from your sensors)
#define n1 0.2 // noise on the 1st measurement component
#define n2 0.1 // noise on the 2nd measurement component 

// model std (~1/inertia). Freedom you give to relieve your evolution equation
#define m1 0.01
#define m2 0.02

KALMAN<Nstate,Nobs> K; // your Kalman filter
BLA::Matrix<Nobs> obs; // observation vector
Adafruit_MPU6050 mpu;
uint8_t i2c_address = MPU6050_I2CADDR_DEFAULT;

void setup(void) {
  Serial.begin(115200);
  while (!Serial)
    delay(10); // will pause Zero, Leonardo, etc until serial console opens

  Serial.println("Adafruit MPU6050 test!");

  // Try to initialize!
  if (!mpu.begin()) {
    Serial.println("Failed to find MPU6050 chip");
    while (1) {
      delay(10);
    }
  }
  Serial.println("MPU6050 Found!");

  mpu.setAccelerometerRange(MPU6050_RANGE_8_G);
  Serial.print("Accelerometer range set to: ");
  switch (mpu.getAccelerometerRange()) {
  case MPU6050_RANGE_2_G:
    Serial.println("+-2G");
    break;
  case MPU6050_RANGE_4_G:
    Serial.println("+-4G");
    break;
  case MPU6050_RANGE_8_G:
    Serial.println("+-8G");
    break;
  case MPU6050_RANGE_16_G:
    Serial.println("+-16G");
    break;
  }
  mpu.setGyroRange(MPU6050_RANGE_500_DEG);
  Serial.print("Gyro range set to: ");
  switch (mpu.getGyroRange()) {
  case MPU6050_RANGE_250_DEG:
    Serial.println("+- 250 deg/s");
    break;
  case MPU6050_RANGE_500_DEG:
    Serial.println("+- 500 deg/s");
    break;
  case MPU6050_RANGE_1000_DEG:
    Serial.println("+- 1000 deg/s");
    break;
  case MPU6050_RANGE_2000_DEG:
    Serial.println("+- 2000 deg/s");
    break;
  }

  mpu.setFilterBandwidth(MPU6050_BAND_5_HZ);
  Serial.print("Filter bandwidth set to: ");
  switch (mpu.getFilterBandwidth()) {
  case MPU6050_BAND_260_HZ:
    Serial.println("260 Hz");
    break;
  case MPU6050_BAND_184_HZ:
    Serial.println("184 Hz");
    break;
  case MPU6050_BAND_94_HZ:
    Serial.println("94 Hz");
    break;
  case MPU6050_BAND_44_HZ:
    Serial.println("44 Hz");
    break;
  case MPU6050_BAND_21_HZ:
    Serial.println("21 Hz");
    break;
  case MPU6050_BAND_10_HZ:
    Serial.println("10 Hz");
    break;
  case MPU6050_BAND_5_HZ:
    Serial.println("5 Hz");
    break;
  }

  Serial.println("");
  K.F = {1.0, 0.0,
         0.0, 1.0};
  // example of measurement matrix. Size is <Nobs,Nstate>
  K.H = {1.0, 0.0,
         0.0, 1.0};
  // example of measurement covariance matrix. Size is <Nobs,Nobs>
  K.R = {n1*n1,   0.0,
           0.0, n2*n2};
  // example of model covariance matrix. Size is <Nstate,Nstate>
  K.Q = {m1*m1,   0.0,
           0.0, m2*m2};
  delay(100);
}

void loop() {
  /* Get new sensor events with the readings */
  sensors_event_t a, g, temp;
  mpu.getEvent(&a, &g, &temp);

  //Serial.print("Temperature: ");
  //Serial.print(temp.temperature);
  //Serial.println(" degC");

  // GRAB MEASUREMENT and WRITE IT INTO 'obs'
  Serial.print(i2c_address);
  
  Serial.print("RX");
  obs(0) = g.gyro.x; // some dummy measurement
  obs(1) = a.acceleration.x; // some dummy measurement
  // APPLY KALMAN FILTER
  K.update(obs);
  // PRINT RESULTS: measures and estimated state
  Serial << K.x;                                       //<----- The Problem here.   
  
  Serial.print("   RY");
  obs(0) = g.gyro.y; // some dummy measurement
  obs(1) = a.acceleration.y; // some dummy measurement
  K.update(obs);
  Serial << K.x;
  
  Serial.print("   RZ");
  obs(0) = g.gyro.z; // some dummy measurement
  obs(1) = a.acceleration.z; // some dummy measurement
  K.update(obs);
  Serial << K.x;

 
  delay(100);
}

这里的问题是在 C# 中我不能使用 Serial << K.x;在 Arduino IDE 中。 c# 的 API 只需要 Serial.print() 或 Serial.write().

Answer 1

关于如何使用这些库不是很明显（但这是复杂的数学，所以这实际上是预期的...）

您可以像这样打印矩阵的内容：

    auto state = K.getxcopy();
    for (int i = 0; i < state.Cols; i++)
    {
        for (int j = 0; j < state.Rows; j++)
        {
            Serial.print(state(i, j));
            Serial.print(" | ");
        }
        Serial.println();
    }

一堆样本可以在 documentation of the BasicLinearAlgebra library 中找到（链接在 Kalman 库的描述中）

卡尔曼滤波 IMU 噪声 c#

Kalman filtering IMU noise c#

c#

filter

arduino

sensors

imu