卡尔曼滤波IMU噪声c#

Question

我有以下问题。我想消除IMU传感器的噪音。我的线索是卡尔曼滤波器。在Arduino IDE中，您可以通过库轻松实现一个。现在我想直接在计算机上用C#解决这个问题，但我在.NET4上找不到可用的库。我在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<lt；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();
}

在BasicLinerAlgebra库的文档中可以找到一组样本(链接到卡尔曼库的描述中(