speedestimationuseLSA
2024-12-30 01:30阅读：
#include < iostream >
#include < cmath >
#include < cstdlib >
#include < ctime >
#include < random >
#include < ceres/ceres.h >
using namespace std;
std::default_random_engine generator;
// 生成仿真点
void generate_points(double* x, double* y, int N)
{
// 高斯随机数生成器
std::normal_distribution pos_dis(0.0, 0.1); //
随机噪声 ,均值0，标准差0.1
std::normal_distribution speed_dis(0.0, 0.05);
// 随机噪声 ,均值0，标准差0.05
// 生成点序列，哪个点，圆心（0，0），半径200，水流速度speed_x, speed_y,
// 附加位置白噪声方差0.1, 附加水流速度噪声方差0.05
for (int i = 0; i < N; i++) {
double t = i + 1;
double noise_x = pos_dis(generator);
double noise_y = pos_dis(generator);
double speed_x = 0.5 + spee
d_dis(generator);
double speed_y = 0.3 + speed_dis(generator);
x[i] = 200 * cos(0.1 * t) + speed_x * t +
noise_x;
y[i] = 200 * sin(0.1 * t) + speed_y * t +
noise_y;
}
}
// 残差函数
struct Residual {
Residual(double x, double y, double t) : x_(x), y_(y) ,
t_(t){}
template < typename T >
bool operator()(const T* const rx, const T* const ry, const
T* const R, const T* const vx, const T* const vy, T* residual)
const {
T x = x_ - t_ * (*vx);
T y = y_ - t_ * (*vy);
T dx = x - *rx;
T dy = y - *ry;
T distance = sqrt(dx * dx + dy * dy) -*R;
residual[0] = distance;
//residual[1] = residual_x;
//residual[2] = residual_y;
return true;
}
double x_;
double y_;
double t_;
};
int main() {
// 初始化随机数种子
generator.seed(time(0));
// 生成点
// const int N = 200;
const int N = 23;
double* x = new double[N];
double* y = new double[N];
generate_points(x, y, N);
// 输出仿真点
std::cout << '------------ sumulate points
----------------------' << std::endl;
for (int i = 0; i < N; ++i)
{
cout << x[i] << ', ' << y[i] <<
std::endl;
}
// 估计参数 ,下为待求量的迭代初值
double rx = 1;
double ry = 10;
double R = 50;
double vx = 0.6;
double vy = 0.5;
// 最小二乘法估算
std::cout << '==== now use LSA to estimate the water
speed =====' << std::endl;
ceres::Problem problem;
for (int i = 0; i < N; i++)
{
problem.AddResidualBlock(new ceres::AutoDiffCostFunction <
Residual,1, 1, 1, 1, 1, 1 > (new Residual(x[i], y[i],i)), NULL,
&rx, &ry, &R, &vx, &vy);
}
ceres::Solver::Options options;
options.max_num_iterations = 1000; //
最大迭代次数
options.function_tolerance = 1e-12;
options.gradient_tolerance = 1e-15;
options.parameter_tolerance = 1e-15;
options.linear_solver_type = ceres::DENSE_QR;
ceres::Solver::Summary summary;
ceres::Solve(options, &problem,
&summary);
// 输出迭代过程
//std::cout << summary.BriefReport() <<
std::endl;
//std::cout << summary.FullReport() <<
std::endl;
// 输出估计结果
std::cout << '-------------- estimated result
-------------------------------------------------------' <<
std::endl;
std::cout << 'estimated center: ' << rx <<
', ' << ry << ', vs the ground truth
(0,0)' << std::endl;
std::cout << 'estimated radiu: ' << R << ',
vs the ground truth 200' <<
std::endl;
std::cout << 'estimated speed of water: ' << vx
<< ', ' << vy << ', vs the ground
truth (0.5,0.3)' << std::endl;
// 比较估算的圆心、半径、干扰速度和前面给定参数的差异
std::cout << 'difference of center: ' << rx - 0
<< ', ' << ry - 0 << std::endl;
std::cout << 'difference of radiu: ' << R - 200
<< std::endl;
std::cout << 'difference of speed: ' << vx - 0.5
<< ', ' << vy - 0.3 << std::endl;
delete[] x;
delete[] y;
return 0;
}
