持之以恒（三）坐标系间的转换关系：最小二乘法

1. 坐标系转换的应用场景

2. 最小二乘法

2.1 简介

线性关系	将n组数据用多项式表示，可通过最小二乘法求解出每个点的最小误差的平方和。
几何意义	高维空间中的一个向量在低维子空间中的投影

最小二乘在线工具：http://www.yunsuan.info/matrixcomputations/solveleastsquares.html

2.2 过程推导

1. 线性推导

拟合方程	y = a + bx	①
点集	（xi, yi）	②
每个点的误差	ei = yi - a - bxi	③
最小误差平方和	S = ∑(ei)² = ∑(yi - a - bxi)²	④

将公式④，分别对a和b求偏导数

通过以上公式推导，可求出a、b、ei的值，可用一个n维单列矩阵表示。

2. 矩阵化
拟合方程可用矩阵化可表示：Ax=B

注：若A和B均为m×n矩阵，则x为n阶正交矩阵。如A和B均为1×3矩阵，则x为3阶正交矩阵。

3. 几何意义：正交投影矩阵

如下图所示，在一个由e1、e2、e3构成的三维坐标系中。v1和v2空间中相交的两个向量。U是由v1和v2构成的平面外的一个向量，P是U在v1和v2构成的平面上的投影。

由图可知，推导如下：

![在这里插入图片描述](https://img-blog.csdnimg.cn/2044b547e8aa404396edf6125283c86a.png

3. C++ 实现

3.1 使用Eigen库

1. Eigen库的配置请参考： 深入浅出（六）Eigen C++ 开源矩阵计算库
2. 最小二乘法

/**
  * @brief 最小二乘法 Ax = B
  *
  * @param xvals 矩阵A，输入矩阵
  * @param yvals 矩阵B，输出矩阵
  * @param order 待输出的系数矩阵的价数
  */
Eigen::VectorXd LeastSquare(const Eigen::MatrixXd xvals, const Eigen::MatrixXd yvals, const int order) {
    //Eigen::MatrixXd::size() = row × col
    assert(xvals.size() == yvals.size());
    //阶数∈[1, xvals.size() - 1]
    assert(order >= 1 && order <= xvals.size() - 1);
    Eigen::MatrixXd A(xvals.size(), order + 1);
    //首列1
    for (unsigned int i = 0; i < xvals.size(); i++) {
        A(i, 0) = 1.0;
    }
    for (unsigned int j = 0; j < xvals.size(); j++) {
        for (int i = 0; i < order; i++) {
            A(j, i + 1) = A(j, i) * xvals(j);
        }
    }
    auto Q = A.householderQr();
    return Q.solve(yvals);
}

3.2 C++自己写

/* 最小二乘法
 * AW=B
 */
bool Mathematical::LeastSquare(const std::vector<double>& xdata,
                 const std::vector<double>& ydata,
                 unsigned int pow_n,
                 std::vector<double>& coefficient,
                 std::string& error_info) {

    if ((xdata.size() != ydata.size())) {
        error_info = "xdata" + std::to_string(xdata.size()) + "与ydata" + std::to_string(ydata.size())+ "数据长度不一致";
        return false;
    }
    else if (pow_n < 1) {
        error_info = "pow_n次幂不能低于1";
        return false;
    }

    //创建矩阵A
    unsigned int a_row = xdata.size(), a_column = pow_n + 1;
    Matrix A = Matrix(a_row, a_column);
    for (unsigned int j = 0, n = pow_n; j < a_column; n--, j++) {
        std::vector<double> a(a_row);
        for (unsigned int i = 0; i < a_row; i++) {
            a[i] = pow(xdata[i], n);
        }
        A.SetColumnVector(j+1, a);
    }
    std::cout << "--- A ---" << std::endl;
    A.Print();

    //创建矩阵B
    unsigned int b_row = ydata.size(), b_column = 1;
    Matrix B = Matrix(b_row, b_column);
    std::vector<double> b(b_row);
    for (unsigned int i = 0; i < a_row; i++) {
        b[i] = ydata[i];
    }
    B.SetColumnVector(1, b);
    std::cout << "--- B ---" << std::endl;
    B.Print();

    Matrix AT = A.Transpose();
    std::cout << "--- AT ---" << std::endl;
    AT.Print();

    //创建矩阵W
    Matrix W1 = (AT ^ A).Inverse() ^ AT ^ B;
    W1.Print();

    coefficient.resize(W1.GetRowNumber());
    for (unsigned int i = 0; i < W1.GetRowNumber(); i++) {
        coefficient[i] = W1.m_matrix[i][0];
    }
    return true;
}


//class Matrix

Matrix::Matrix(unsigned int row, unsigned int column) : m_row(row), m_column(column){
    m_matrix.resize(m_row);
    for (unsigned int i = 0; i < m_row; i++) {
        m_matrix[i].resize(m_column);
    }
    //检查矩阵是否是方阵
    is_square = m_row == m_column ? true : false;
}

//矩阵的行元素列表,从1开始数
std::vector<double> Matrix::GetRowVector(const unsigned int row) {
    if (row > m_row || row < 1) {
        return {};
    }
    return m_matrix[row - 1];
}
//矩阵的列元素列表
std::vector<double> Matrix::GetColumnVector(const unsigned int column) {
    if (column > m_column || column < 1) {
        return {};
    }
    std::vector<double> column_vec(m_row);
    unsigned int count = 0;
    for (unsigned int i = 0; i < m_row; i++) {
        for (unsigned int j = 0; j < m_column; j++) {
            if (j == (column - 1)) {
                column_vec[count++] = m_matrix[i][j];
            }
        }
    }
    return column_vec;
}
bool Matrix::SetRowVector(const unsigned int row, const std::vector<double>& row_vector) {
    if (row > m_row || row < 1 || row_vector.size() != m_column) {
        throw "请检查向量，设置失败！！！";
        return false;
    }
    m_matrix[row-1] = row_vector;
    return true;
}
bool Matrix::SetColumnVector(const unsigned int column, const std::vector<double>& column_vector) {
    if (column > m_column || column < 1 || column_vector.size() != m_row) {
        throw "请检查向量，设置失败！！！";
        return false;
    }
    unsigned int count = 0;
    for (unsigned int i = 0; i < m_row; i++) {
        for (unsigned int j = 0; j < m_column; j++) {
            if (j == (column - 1)) {
                m_matrix[i][j] = column_vector[count++];
            }
        }
    }
    return true;
}

//转置
Matrix Matrix::Transpose() {
    Matrix mat(m_column, m_row);
    for (unsigned int i = 0; i < m_row; i++) {
        for (unsigned int j = 0; j < m_column; j++) {
            mat.m_matrix[j][i] = m_matrix[i][j];
        }
    }
    return mat;
}

//余子式 从0开始数
Matrix Matrix::Cofactor(const unsigned int r, const unsigned int c) {
    Matrix cofactor = Matrix(m_row - 1, m_column - 1);
    unsigned int m = 0, n = 0;
    for (unsigned int i = 0; i < m_row; i++) {
        if (i == (r - 1)) {
            continue;
        }
        for (unsigned int j = 0; j < m_column; j++) {
            if (j == (c - 1)) {
                continue;
            }
            cofactor.m_matrix[m][n++] = m_matrix[i][j];
        }
        m++, n = 0;
    }
    return cofactor;
}

//代数余子式 从1开始数
double Matrix::AlgebraicCofactor(const unsigned int r, const unsigned int c) {
    double k = ((r + c) % 2) ? (-1.0) : (1.0);
    return (Cofactor(r, c).Value() * k);
}

//伴随矩阵
Matrix Matrix::Adjoint() {
    Matrix adjoint = Matrix(m_row, m_column);
    for (unsigned int i = 0; i < m_row; i++) {
        for (unsigned int j = 0; j < m_column; j++) {
            adjoint.m_matrix[i][j] = AlgebraicCofactor(i + 1, j + 1);
        }
    }
    return adjoint.Transpose();
}

//方阵的逆矩阵
Matrix Matrix::Inverse() {
    if (!is_square) {
        throw "矩阵不是方阵，无逆矩阵！！！";
        return {};
    }
    if (m_row > 3 || m_column > 3) {
        throw "大于3阶逆矩阵暂未实现！！！";
        return {};
    }
    Matrix A_adjoint = Adjoint();
    A_adjoint / Value();
    return A_adjoint;
}

//方阵行列式的值
double Matrix::Value() {
    if (!is_square) {
        throw "矩阵不是方阵，不能计算行列式的值！！！";
    }
    if (m_row == 1) {
        return m_matrix[0][0];
    }
    else if (m_row == 2) {
        return (m_matrix[0][0] * m_matrix[1][1] - m_matrix[1][0] * m_matrix[0][1]);
    }
    else if (m_row == 3) {
        return (m_matrix[0][0] * m_matrix[1][1] * m_matrix[2][2] +
                m_matrix[0][1] * m_matrix[1][2] * m_matrix[2][0] +
                m_matrix[0][2] * m_matrix[1][0] * m_matrix[2][1] -
                m_matrix[0][0] * m_matrix[1][2] * m_matrix[2][1] -
                m_matrix[0][1] * m_matrix[1][0] * m_matrix[2][2] -
                m_matrix[0][2] * m_matrix[1][1] * m_matrix[2][0]);
    }
    else {
        throw "暂不支持！！！";
    }
}

//重载[],从0开始计数
std::vector<double> Matrix::operator[] (unsigned int column) {
    return GetRowVector(column + 1);
}

//重载=
bool Matrix::operator= (const Matrix& mat) {
    unsigned int row_size = mat.m_matrix[0].size(), column_size = mat.m_matrix.size();
    for (unsigned int i = 0; i < row_size; i++) {
        for (unsigned int j = 0; j < column_size; j++) {
            m_matrix[i][j] = mat.m_matrix[i][j];
        }
    }
    m_row = row_size, m_column = column_size;
    return true;
}

//重载+
bool Matrix::operator+ (const Matrix& mat) {
    if (mat.m_row != m_row || mat.m_column != m_column) {
        throw "矩阵不等价，不能进行加法运算！！！";
        return false;
    }
    for (unsigned int i = 0; i < m_row; i++) {
        for (unsigned int j = 0; j < m_column; j++) {
            m_matrix[i][j] += mat.m_matrix[i][j];
        }
    }
    return true;
}
bool Matrix::operator- (const Matrix& mat) {
    if (mat.m_row != m_row || mat.m_column != m_column) {
        throw "矩阵不等价，不能进行减法运算！！！";
        return false;
    }
    for (unsigned int i = 0; i < m_row; i++) {
        for (unsigned int j = 0; j < m_column; j++) {
            m_matrix[i][j] -= mat.m_matrix[i][j];
        }
    }
    return true;
}

//重载* 数乘
bool Matrix::operator* (const double k) {
    for (unsigned int i = 0; i < m_row; i++) {
        for (unsigned int j = 0; j < m_column; j++) {
            m_matrix[i][j] *= k;
        }
    }
    return true;
}

//重载/ 数除
bool Matrix::operator/ (const double k) {
    if (k == 0) {
        return false;
    }
    for (unsigned int i = 0; i < m_row; i++) {
        for (unsigned int j = 0; j < m_column; j++) {
            m_matrix[i][j] /= k;
        }
    }
    return true;
}
//重载^ 点乘
//求两个矩阵相乘，A * B（mat）
Matrix Matrix::operator^ (const Matrix& mat) {
    if (m_column != mat.m_row) {
        throw "矩阵行列不对称，不能相乘！！！";
        return {};
    }
    Matrix square = Matrix(m_row, mat.m_column);
    for (unsigned int i = 0; i < square.m_row; i++) {
        for (unsigned int j = 0; j < square.m_column; j++) {
            for (unsigned int m = 0; m < mat.m_row; m++) {
                //取矩阵A，行数组     m_matrix[i][x]
                //取矩阵B，列数组 mat.m_matrix[x][j]
                square.m_matrix[i][j] += m_matrix[i][m] * mat.m_matrix[m][j];
            }
        }
    }
    return square;
}

//矩阵打印工具
void Matrix::Print() {
    unsigned int row = GetRowNumber(), column = GetColumnNumber();
    if (row < 1 || column < 1) {
        throw ("Matrix 矩阵不正确，请检查矩阵！！！");
    }
    for (unsigned int i = 0; i < row; i++) {
        for (unsigned int j = 0; j < column; j++) {
            std::cout << (j == 0 ? "[" : "") << (m_matrix[i][j] >= 0 ? " ": "" )
                      << m_matrix[i][j] << (j < column - 1 ? "\t" : "]\n");
        }
    }
}

4. MATLAB实现

clear  %清理工作区变量
clc    %清空历史命令行窗口中的内容，但是其变量都不会消失
close  %关闭图形窗口

%多项式最高项次数
n=2;
xdata=[0.5,1.0,1.5,2.0,2.5,3.0];
ydata=[1.75,2.45,3.81,4.80,7.00,8.60];
%多项式函数拟合，输出n+1个多项式系数
p=polyfit(xdata,ydata,n)
x1=0.5:0.5:3;
%多项式在x处的值y计算
y1=polyval(p,x1)
plot(xdata,ydata,'*r',x1,y1,'-b')

%输出多项式-MATLAB函数
poly2sym(p)

%输出多项式-打印
b='x^';
fprintf('\nfx=')
for i=1 : (n+1)
    if p(i) > 0
        fprintf('+%f*%s%d',p(i),b,n+1-i)
    else if  p(i) < 0
           fprintf('%f*%s%d',p(i),b,n+1-i)
        else if p(i) == 0
               fprintf('   ')
            end
        end
    end
end
fprintf('\n')