做网站的大公司,个人网站 创意,扫一扫网页版在线使用,个人博客网站的建设结构图通过 Eigen 矩阵运算实现线性卡尔曼滤波。模拟的是一辆带火箭发动机的汽车#xff0c;一开始沿着X轴匀速运动#xff0c;到X轴中间开启火箭发动机匀加速向Y轴起飞。同理可以仿真#xff08;x,y,z,yaw,pitch,raw#xff09; 6自由度的真实飞行情况
#include iostream一开始沿着X轴匀速运动到X轴中间开启火箭发动机匀加速向Y轴起飞。同理可以仿真x,y,z,yaw,pitch,raw 6自由度的真实飞行情况
#include iostream
#include Eigen/LU
#include Eigen/coreusing namespace Eigen;using Matrix6f Eigen::Matrixfloat, 6, 6;
using Vector6f Eigen::Matrixfloat, 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;}Matrixfloat, 6, 2 getB(float dt){Matrixfloat, 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 Matrixfloat, 6, 2 B, const Vector2f u){fx A * x B * u;fP A * P * A.transpose() Q;}void calibration(const Vector2f z){Matrixfloat, 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; // 观测协方差Matrixfloat, 2, 6 H; // 隐状态到观测状态的映射
};int main()
{// 测试数据float a 1.1;std::vectorVector2f real;std::vectorVector2f 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);Matrixfloat, 6, 2 B filter.getB(dt);Vector2f u;u 0, 0;std::vectorVector2f 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;}
}
