问题描述

对于曲线，其中是曲线参数，噪声项。

假设左右有个观测数据点，希望能得到曲线参数，可以构造一个非线性最小二乘问题来估计参数：

将误差定义为：，则可以得到雅可比矩阵：

其中，偏导表达式如下：

那么高斯-牛顿法中的增量方程可以构造如下：

工程建设

采用Ubuntu20.04+CLion进行开发，主要使用Eigen库进行矩阵计算。

工程结构

├── CMakeLists.txt
├── include
│   └── main.h
└── src
    ├── CMakeLists.txt
    └── main.cpp

CMake配置

主要使用了OpenCV库生成数据点，Eigen库用于矩阵计算。根目录下CMakeLists.txt文件内容如下：

# cmake version
cmake_minimum_required(VERSION 3.21)
# project name
project(Study)
# cpp version
set(CMAKE_CXX_STANDARD 14)
# eigen
include_directories("/usr/include/eigen3")
# opencv
find_package(OpenCV REQUIRED)
include_directories(${OpenCV_INCLUDE_DIRS})
# incldue
include_directories(include)
# src
add_subdirectory(src)

src下CMakeLists.txt文件内容如下：

# exec
add_executable(Study main.cpp)
# link opencv
target_link_libraries(Study ${OpenCV_LIBRARIES})

头文件配置

include目录下头文件main.h内容如下：

//
// Created by jasonli on 2022/2/12.
//

#ifndef STUDY_MAIN_H
#define STUDY_MAIN_H

#include <iostream>
#include <cmath>
#include <unistd.h>
//  Eigen
#include <Eigen/Core>
#include <Eigen/Dense>
//  OpenCV
#include <opencv2/opencv.hpp>
//  namespace
using namespace std;
using namespace Eigen;


#endif //STUDY_MAIN_H

源文件配置

src下源文件main.cpp导入头文件：

#include "main.h"

int main()
{

    return 0;
}

代码

整体代码和注释如下：

#include "main.h"

int main()
{
    /*--------  初始参数配置  --------*/
    //  实际曲线参数
    double ar = 1.0, br = 1.0, cr = 1.0;
    //  估计曲线参数初始值
    double ae = 2.0, be = 1.0, ce = 5.0;
    //  采样观测数据点个数
    int N = 100;
    //  噪声标准差及其倒数
    double w_sigma = 1.0;
    double inv_sigma = 1.0 / w_sigma;
    //  随机数生成器
    cv::RNG rng;

    /*--------  观测数据生成  --------*/
    vector<double> x_data, y_data;
    for(int i=0; i<N; i++){
        double x = i / 100.0;
        x_data.push_back(x);
        y_data.push_back(exp(ar * x * x +br * x + cr) + rng.gaussian(w_sigma*w_sigma));
    }

    /*--------  Gauss-Newton    --------*/
    //  迭代次数
    int iterations = 100;
    //  迭代cost以及上一次迭代cost
    double cost, lastCost = 0;

    //  开始求解时间t1
    chrono::steady_clock::time_point t1 = chrono::steady_clock::now();
    for(int iter=0; iter < iterations; iter++){
        //  增量方程左侧值初始化：Hessian = J^T W^{-1} J in Gauss-Newton
        Matrix3d H = Matrix3d::Zero();
        //  增量方程右侧值初始化：g = - W^{-1} ei J
        Vector3d g = Vector3d::Zero();
        //  估计同真实的差距值，归零化
        cost = 0;

        for(int i=0; i < N; i++){
            //  获取第i个数据点
            double xi = x_data[i], yi = y_data[i];
            //  误差
            double ei = yi - exp(ae * xi * xi + be * xi + ce);
            //  雅克比矩阵
            Vector3d J;
            J[0] = - xi * xi * exp(ae * xi * xi + be * xi + ce);
            J[1] = - xi * exp(ae * xi * xi + be * xi + ce);
            J[2] = - exp(ae * xi * xi + be * xi + ce);
            //  增量方程系数
            H += inv_sigma * inv_sigma * J * J.transpose();
            g += -inv_sigma * inv_sigma * ei * J;
            //  差距
            cost += ei * ei;
        }
        //  求解增量方程Hdx=g
        Vector3d dx = H.ldlt().solve(g);
        if(isnan(dx[0])){
            cout << "result is nan !" << endl;
            break;
        }

        if(iter > 0 && cost >= lastCost){
            cout << "cost:" << cost << ">= last cost :" << lastCost <<", break." << endl;
            break;
        }
        //  更新估计值
        ae += dx[0];
        be += dx[1];
        ce += dx[2];
        //  更新差距
        lastCost = cost;

        cout << "total cost:" << cost << ", \t\t update:" << dx.transpose() << "\t\t estimated params:" << ae << "," << be << "," << ce << endl;

    }

    //  结束求解时间t2
    chrono::steady_clock::time_point t2 = chrono::steady_clock::now();
    //  求解花费时间t2 - t1
    chrono::duration<double> time_used = chrono::duration_cast<chrono::duration<double>>(t2 - t1);
    cout << "solve time cost = " << time_used.count() << "seconds. " << endl;

    //  最终估计量
    cout << "estimated abc = " << ae << " , " << be << " , " << ce << endl;

    return 0;
}

输出

输出如下：

total cost:9.26353e+07, 		 update:-0.00563613   -0.006202   -0.981441		 estimated params:1.99436,0.993798,4.01856
total cost:1.23782e+07, 		 update:-0.0154356 -0.0158777  -0.950534		 estimated params:1.97893,0.97792,3.06802
total cost:1.61827e+06, 		 update:-0.0425927 -0.0364985   -0.87242		 estimated params:1.93634,0.941422,2.19561
total cost:199423, 		 update:  -0.11697 -0.0600174  -0.697204		 estimated params:1.81937,0.881404,1.4984
total cost:20992.7, 		 update:  -0.287499 -0.00580094   -0.403671		 estimated params:1.53187,0.875603,1.09473
total cost:1519.46, 		 update:-0.437282  0.186207 -0.135058		 estimated params:1.09458,1.06181,0.959671
total cost:132.709, 		 update: -0.205976   0.142068 -0.0272031		 estimated params:0.888608,1.20388,0.932468
total cost:102.041, 		 update: -0.0110366  0.00856986 -0.00121983		 estimated params:0.877572,1.21245,0.931248
total cost:102.004, 		 update: 7.74339e-05 -9.85241e-05  2.39198e-05		 estimated params:0.877649,1.21235,0.931272
total cost:102.004, 		 update:-3.05581e-07  3.32264e-07 -5.91727e-08		 estimated params:0.877649,1.21235,0.931272
total cost:102.004, 		 update: 1.77986e-09 -2.04038e-09  4.27718e-10		 estimated params:0.877649,1.21235,0.931272
cost:102.004>= last cost :102.004, break.
solve time cost = 0.00442306seconds. 
estimated abc = 0.877649 , 1.21235 , 0.931272