卡尔曼滤波

通过 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,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);// 测量不确定度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, ayMatrix6f 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);Vector2f d;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;}
}