armadillo库小案例

#include <stdio.h>
#include <stdlib.h>
#include <string.h>
#include <iostream>
#include <armadillo>
using namespace std;
using namespace arma;

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)
{
    mat X1(1, 4), F1(4, 4), P(4, 4), Q(4, 4), H(4, 4), R(4, 4);
    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;
    for (int j = 0; j < 16; j++)
    {
      F1[j] = 0;
    }
    for (int k = 0; k < 4; k++)
    {
      F1[k * 5] = 1;
    }
    F1[2] = F1[7] = 0.25;
    mat F = F1.t();

    for (int i = 0; i < 16; i++)
    {
      P[i] = 0;
    }
    P[0] = P[5] = 0.3;
    P[10] = P[15] = 0.1;

    for (int i = 0; i < 16; i++)
    {
      Q[i] = 0;
    }
    for (int i = 0; i < 4; i++)
    {
      Q[i * 5] = 0.3;
    }

    for (int i = 0; i < 16; i++)
    {
      H[i] = 0;
    }
    H[0] = H[5] = 1;

    for (int i = 0; i < 16; i++)
    {
      R[i] = 0;
    }
    R[0] = R[5] = 2;
    R[10] = R[15] = 0.1;

    mat I = eye(4, 4);

    X1[0] = f_in.x;
    X1[1] = f_in.y;
    X1[2] = X1[3] = 0;
    mat X = X1.t();
    if(fabs(cx - lx) > 5 || fabs(cy  - ly) > 5 || (cx == 0 && cy == 0))
        {
            printf("abs > 5 or x = y = 0\n");
            return -1;
        }

    if (fabs(cx - lx) < 5 && fabs(cy  - ly) < 5)
    {
      mat LX = X;
      mat X_ = F * LX;
      mat P_ = F * P * F.t() + Q;
      mat K = P_ * H.t() * inv(H * P_ * H.t() + R);
      X1[0] = cx;
      X1[1] = cy;
      X1[2] = X1[3] = 0;
      mat Z = X1.t();
      X = X_ + K * (Z - H * X_);
      P = (I - K * H) * P_;
      out->xout = X[0];
      out->yout = X[1];
    }
    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;
}