卡尔曼滤波

通过 Eigen 矩阵运算实现线性卡尔曼滤波。模拟的是一辆带火箭发动机的汽车，一开始沿着X轴匀速运动，到X轴中间开启火箭发动机匀加速向Y轴起飞。同理可以仿真（x,y,z,yaw,pitch,raw） 6自由度的真实飞行情况

#include <iostream>
#include <Eigen/LU>
#include <Eigen/core>


using namespace Eigen;

using Matrix6f = Eigen::Matrix<float, 6, 6>;
using Vector6f = Eigen::Matrix<float, 6, 1>;


class CalmanFilter
{
public:
    CalmanFilter()
    {
        // 初始状态不确定度
        P << std::pow(0,2), 0, 0, 0, 0, 0, 
            0, std::pow(1,2), 0, 0, 0, 0,
            0, 0, std::pow(0.1,2), 0, 0, 0,
            0, 0, 0, std::pow(0,2), 0, 0,
            0, 0, 0, 0, std::pow(1,2), 0,
            0, 0, 0, 0, 0, std::pow(0.1, 2);

        // 环境不确定度
        Q << std::pow(0,2), 0, 0, 0, 0, 0, 
            0, std::pow(0,2), 0, 0, 0, 0,
            0, 0, std::pow(0,2), 0, 0, 0, // 当外部环境导致X轴加速度变化时这里应相应配置
            0, 0, 0, std::pow(0,2), 0, 0,
            0, 0, 0, 0, std::pow(0,2), 0,
            0, 0, 0, 0, 0, std::pow(0,2); // 当外部环境导致Y轴加速度变化时这里应相应配置

        // 测量不确定度
        float xVariance = 5;
        float yVariance = 5;
        R << std::pow(xVariance,2), 0,
            0, std::pow(xVariance,2);

        // 隐变量到观测变量的映射
        H << 1, 0, 0, 0, 0, 0,
            0, 0, 0, 1, 0, 0;
    }

    void init(const Vector6f& x)
    {
        this->x = x;
        this->fx = x;
    }

    Matrix6f getA(float dt)
    {
        Matrix6f A;
        A << 1, dt, 0.5*dt*dt, 0, 0, 0, 
            0, 1, dt, 0, 0, 0,
            0, 0, 1, 0, 0, 0,
            0, 0, 0, 1, dt, 0.5*dt*dt,
            0, 0, 0, 0, 1, dt,
            0, 0, 0, 0, 0, 1;
        return A;
    }

    Matrix<float, 6, 2> getB(float dt)
    {
        Matrix<float, 6, 2> B;
        B << 0.5*dt*dt, 0,
            dt, 0,
            0, 0,
            0, 0.5*dt*dt,
            0, dt,
            0, 0;
        return B;
    }

    void forcast(const Matrix6f& A, const Matrix<float, 6, 2>& B, const Vector2f& u)
    {
        fx = A * x + B * u;
        fP = A * P * A.transpose() + Q;
    }

    void calibration(const Vector2f& z)
    {
        Matrix<float, 6, 2> K = P * H.transpose() * (H * P * H.transpose() + R).inverse();
        x = fx + (K * (z - H * fx));
        P = fP - K * H * fP;
    }

    Vector2f getEstimation() const
    {
        return H * x;
    }

private:
    Vector6f fx; // x, vx, ax, y, vy, ay
    Matrix6f fP; // fx 的协方差
    Matrix6f Q; // 环境干扰协方差

    Vector6f x; // fx 校准值
    Matrix6f P; // fP 校准值
    Matrix2f R; // 观测协方差

    Matrix<float, 2, 6> H; // 隐状态到观测状态的映射
};

int main()
{
    // 测试数据
    float a = 1.1;
    std::vector<Vector2f> real;
    std::vector<Vector2f> observations;
    for (int i = 0; i < 100; ++i) {
        float x = i;
        float y = 0;
        if (i >= 50) {
            float t = (i - 50)/10.;
            y = 0.5 * a * t * t;
        }
        real.emplace_back(x, y);
        observations.emplace_back(x + rand() % 5, y + rand() % 5);
    }


    CalmanFilter filter;
    Vector6f initState;
    initState << observations[0][0], 0, 0, observations[0][1], 0, 0;
    filter.init(initState);

    float dt = 0.1;
    Matrix6f A =  filter.getA(dt);
    Matrix<float, 6, 2> B = filter.getB(dt);

    Vector2f u;
    u << 0, 0;

    std::vector<Vector2f> estimations;
    for (int i = 0; i < observations.size(); ++i) {
        if (i >= 50)
            u[1] = a;

        filter.forcast(A, B, u);

        filter.calibration(observations[i]);

        Vector2f est = filter.getEstimation();
        estimations.push_back(est);
    }

    for (int i = 0; i < estimations.size(); ++i) {
        std::cout << i << " est: " << estimations[i] << " real: " << real[i] << " obs: " << observations[i] << std::endl;
    }
}