1. 考虑线性方程组Ax=b,其中A∈Rm×n且rank(A)=min{m,n}.
1)当m>n, 推导求解最小二乘解.xlsq=min||Ax-b||2，需要写出细致的每一步的推导过程
推导步骤：
2. 假设有三个WIFI热点，位置分别在(x1,y1), (x2,y2), (x3,y3), 移动端测量到每一个热点的距离L1,L2和L3，要求解移动端的位置.
需要写一个C/C++/JAVA的程序，（基于第一题的结论自己实现求解过程，可以调用基础的数学矩阵库，但是不允许调用Eigen等优化库），最终的程序需要可以运行，输入3个热点坐标和测量距离，可以得到正确的移动端位置。
下面是优化的问题解法。 其实还有更简单的解法，因为只有三个wifi热点，列三个方程，1，2，3，消掉其中的二次项，有点像高中的几何题，但这种方法只适合有三个wifi热点的情况。优化问题的解法能适合有更多wifi观测的情况。
#include <iostream>
#include <vector>
#include <opencv2/opencv.hpp>
using namespace std;
/**
* @brief 计算误差向量
* @param wifi_positions wifi的位置
* @param term_loc 终端的位置
* @param distances 观测的终端到wifi的位置
* @param error_vector 存储误差向量（输出参数）
* @param error_mean 存储平均误差用于迭代结束条件判断(输出参数）
*/
void CalErrorVector(const vector<cv::Point2f>& wifi_positions, const cv::Point2f& term_loc, const vector<float>& distances, cv::Mat& error_vector, float& error_mean)
{
if (wifi_positions.size() != error_vector.rows||wifi_positions.size()!=distances.size()||error_vector.rows!=distances.size())
{
std::cout << "Please chack the dims of data" << std::endl;
return;
}
cv::Point2f diff;
error_mean = 0;
for (int i = 0; i < wifi_positions.size(); i++)
{
diff = term_loc - wifi_positions[i];
float e = diff.x * diff.x + diff.y * diff.y - distances[i] * distances[i];
error_vector.at<float>(i, 0) = e;
error_mean += std::fabs(e);
}
error_mean /= int(wifi_positions.size());
}
/**
* @brief 计算雅可比矩阵
* @param wifi_positions wifi的位置
* @param term_loc 终端的位置
* @param jac_matrix 存储雅可比矩阵（输出参数）
*/
void CalJacMatrix(const vector<cv::Point2f>& wifi_positions, const cv::Point2f& term_loc, cv::Mat& jac_matrix)
{
if (wifi_positions.size() != jac_matrix.rows)
{
std::cout << "Please chack the dims of data" << std::endl;
return;
}
cv::Point2f diff;
for (int i = 0; i < wifi_positions.size(); i++)
{
diff = term_loc - wifi_positions[i];
jac_matrix.at<float>(i, 0) = 2*diff.x;
jac_matrix.at<float>(i, 1) = 2*diff.y;
}
}
/**
* @brief 优化函数
* @param wifi_positions wifi的位置
* @param distances 观测的终端到wifi的位置
* @param term_loc 终端的位置（输出参数）
*/
void OptimizePosition(const vector<cv::Point2f>& wifi_potisions, const vector<float>& distances, cv::Point2f& term_loc)
{
if (wifi_potisions.size() < 3)
{
std::cout << " The number of points should be greater than 3" << std::endl;
return;
}
cv::Point2f init_loc(0.0, 0.0);
for (int i = 0; i < wifi_potisions.size(); i++)
{
init_loc += wifi_potisions[i];
}
init_loc /= int(wifi_potisions.size());
//定义误差向量
cv::Mat error_vector;
float error_mean;
error_vector.create(wifi_potisions.size(), 1, CV_32F);
CalErrorVector(wifi_potisions, init_loc, distances, error_vector, error_mean);
//定义雅可比矩阵
cv::Mat jac_mat;
jac_mat.create(wifi_potisions.size(), 2, CV_32F);
cv::Mat update;
//当平均误差>0.0001执行while循环
while (std::fabs(error_mean) > 0.0001)
{
//计算雅可比矩阵
CalJacMatrix(wifi_potisions, init_loc, jac_mat);
cv::Mat jac_mat_t;
cv::transpose(jac_mat, jac_mat_t);
cv::Mat H = jac_mat_t * jac_mat;
cv::Mat H_inv;
cv::invert(H, H_inv);
//std::cout << H * H_inv << std::endl;
cv::Mat g = - jac_mat_t * error_vector;
update = H_inv * g;
init_loc.x += update.at<float>(0, 0);
init_loc.y += update.at<float>(1, 0);
CalErrorVector(wifi_potisions, init_loc, distances, error_vector, error_mean);
//std::cout << "loc: " << init_loc << std::endl;
}
term_loc = init_loc;
}
int main()
{
//构造测试数据
vector<cv::Point2f> wifi_positions = { cv::Point2f(1.0,0),cv::Point2f(0.0,1.0),cv::Point2f(1.0,1.0) };
vector<float> distances = { 1.0,1.0,1.41421 };
cv::Point2f term_loc_gt(0.0, 0.0), term_loc(0.0, 0.0);
OptimizePosition(wifi_positions, distances, term_loc);
//结果检验，如果diff的x和y成员接近0，则说明上面的代码是OK的
auto diff = term_loc_gt - term_loc;
std::cout << "diff: " << diff << std::endl;
return 0;
}