#include <stdio.h>
#include <string.h>
#include <stdlib.h>
#include <iostream>
#include "Eigen/Eigen"
using namespace std;
using namespace Eigen;
typedef struct _coor_fin
{
double x;
double y;
}coor_fin;
typedef struct _coor_lin
{
double x;
double y;
}coor_lin;
typedef struct _coor_cin
{
double x;
double y;
}coor_cin;
typedef struct _coor_out
{
double xout;
double yout;
}coor_out;
/*
*函数功能:坐标平滑矫正
*函数入参:coor_fin:基准坐标
* l_in:上一次坐标
* c_in:本次坐标
*函数出参 coor_out 矫正后的坐标
*函数返回值:成功返回 0, 失败返回 -1
*/
int correct_position(coor_fin f_in, coor_lin l_in, coor_cin c_in, coor_out *out)
{
MatrixXf X(1, 4), X1(1, 4), F(4, 4), F1(4, 4), P(4, 4), Q(4, 4), H(4, 4), R(4, 4), I(4, 4);
F << 1,0,0.25,0,0,1,0,0.25,0,0,1,0,0,0,0,1;
P << 0.3,0,0,0,0,0.3,0,0,0,0,0.1,0,0,0,0,0.1;
Q << 0.3,0,0,0,0,0.3,0,0,0,0,0.3,0,0,0,0,0.3;
H << 1,0,0,0,0,1,0,0,0,0,0,0,0,0,0,0;
R << 2,0,0,0,0,2,0,0,0,0,0.1,0,0,0,0,0.1;
I << 1,0,0,0,0,1,0,0,0,0,1,0,0,0,0,1;
double fx = f_in.x;
double fy = f_in.y;
double lx = l_in.x;
double ly = l_in.y;
double cx = c_in.x;
double cy = c_in.y;
X1 << f_in.x, f_in.y, 0, 0;
X = X1.transpose();
if(fabs(cx - lx) > 5 || fabs(cy - ly) > 5 || (cx == 0 && cy == 0))
{
printf("fabs > 5 or x = y = 0\n");
return -1;
}
if (fabs(cx - lx) < 5 && fabs(cy - ly) < 5)
{
MatrixXf LX = X;
MatrixXf X_ = F * LX;
MatrixXf P_ = F * P * F.transpose() + Q;
MatrixXf MN = H * P_ * H.transpose() + R;
MatrixXf K = P_ * H.transpose() * MN.inverse();
X1 << cx, cy, 0, 0;
MatrixXf Z = X1.transpose();
X = X_ + K * (Z - H * X_);
P = (I - K * H) * P_;
out->xout = X(0,0);
out->yout = X(1,0);
}
return 0;
}
int main()
{
coor_fin f_in;
f_in.x = 43.432743;
f_in.y = 0.229601;
coor_lin l_in;
l_in.x = 43.374115;
l_in.y = 0.002367;
coor_cin c_in;
c_in.x = 43.469775;
c_in.y = 0.240061;
coor_out out;
int ret = correct_position(f_in, l_in, c_in, &out);
if(ret == -1)
{
printf("correct_position error\n");
return -1;
}
printf("out_x = %f\n", out.xout);
printf("out_y = %f\n", out.yout);
return 0;
}
Eigen库小案例
最新推荐文章于 2025-04-26 20:52:37 发布