头文件

/*
  Copyright(C) 2019- White Noise Team

  One dimensional Kalman Filter implementation for
  use with BNO055 IMU to filter noisy gyro and
  accelerometer measurements.

  This software is distributed under the MIT License

  This file includes the headers of the Kalman Filter API
*/

class Kalman {
public:
  float R, Q, A, B, C;
  float cov;
  float x; // Signal without noise
  bool init;

  Kalman(float r, float q, float a, float b, float c);

  float filter(float z, float u);

  float predict(float u);

  float uncertainty();

};

Cpp

/*
  Copyright(C) 2019- White Noise Team

  One dimensional Kalman Filter implementation for
  use with BNO055 IMU to filter noisy gyro and
  accelerometer measurements.

  This software is distributed under the MIT License

  This file includes the source of the Kalman Filter API
*/

#include "Kalman.h"

Kalman::Kalman(float r, float q, float a, float b, float c) {
  R = r;
  Q = q;
  A = a;
  B = b;
  C = c;
  init = false;
}

float Kalman::filter(float z, float u) {
  if (!init) {
    x = 1 / C * z;
    cov = Q / (C * C);
    init = true;
  }

  else {
    float pred = predict(u);
    float p_cov = uncertainty();

    float K = p_cov * C / (C * C * p_cov + Q);

    x = pred + K * (z - C * pred);
    cov = p_cov - K * C * p_cov;

  }

  return x;

}

float Kalman::predict(float u) {
  return A * x + B * u;
}

float Kalman::uncertainty() {
  return A * A * cov + R;
}

example

#include "Kalman.h"
#include <cstdlib>
#include <iostream>

using namespace std;

float randn() {
  return (float)rand() / (float) RAND_MAX;
}

int main() {
  Kalman *F = new Kalman(1, 1, 1, 1, 1);
  float z;
  for (int i = 0; i < 1000; i++) {
    z = randn();
    F->filter(z, 0);
    cout << "t: " << i << " x: " << F->x << " z: " << z << endl;

  }
}