视觉SLAM作业（四） 相机模型与非线性优化

视觉SLAM作业（四） 相机模型与非线性优化

一 图像去畸变

现实生活中的图像总存在畸变。原则上来说，针孔透视相机应该将三维世界中的直线投影成直线，但是当我们使用广角和鱼眼镜头时，由于畸变的原因，直线在图像里看起来是扭曲的。本次作业，你将尝试如何对一张图像去畸变，得到畸变前的图像。

图1 是本次习题的测试图像（code/test.png），来自EuRoC 数据集[1]。可以明显看到实际的柱子、箱子的直线边缘在图像中被扭曲成了曲线。这就是由相机畸变造成的。根据我们在课上的介绍，畸变前后的坐标变换为：
x d i s t o r t e d = x ( 1 + k 1 r 2 + k 2 r 4 ) + 2 p 1 x y + p 2 ( r 2 + 2 x 2 ) y d i s t o r t e d = y ( 1 + k 1 r 2 + k 2 r 4 ) + p 1 ( r 2 + 2 y 2 ) + 2 p 2 x y x_{distorted} = x(1 + k_1r^2 + k_2r^4)+ 2p_1xy + p_2(r^2 + 2x^2)\\ y_{distorted} = y(1 + k_1r^2 + k_2r^4)+ p_1(r^2 + 2y^2)+ 2p_2xy xdistorted​=x(1+k1​r2+k2​r4)+2p1​xy+p2​(r2+2x2)ydistorted​=y(1+k1​r2+k2​r4)+p1​(r2+2y2)+2p2​xy
其中x; y 为去畸变后的坐标， x d i s t o r t e d x_{distorted} xdistorted​，$ y_{distroted}$ 为去畸变前的坐标。

现给定参数：
k 1 = 0.28340811 ; k 2 = 0.07395907 ; p 1 = 0.00019359 ; p 2 = 1.76187114 e − 5 : k_1= 0.28340811; k2 = 0.07395907; p_1 = 0.00019359; p_2 = 1.76187114e^{-5}: k1​=0.28340811;k2=0.07395907;p1​=0.00019359;p2​=1.76187114e−5:
以及相机内参
f x = 458.654 ; f y = 457.296 ; c x = 367.215 ; c y = 248.375 : f_x = 458.654; f_y = 457.296; c_x = 367.215; c_y = 248.375: fx​=458.654;fy​=457.296;cx​=367.215;cy​=248.375:
请根据undistort_image.cpp 文件中内容，完成对该图像的去畸变操作。

答： 去畸变过程主要包括以下步骤：

1. 将图像的像素坐标系通过内参矩阵转换到相机归一化坐标系
   x = ( u − c x ) / f x y = ( v − c y ) / f y x = (u-c_x)/f_x\\ y = (v-c_y)/f_y x=(u−cx​)/fx​y=(v−cy​)/fy​

2. 在相机坐标系下进行去畸变操作
   r = x 2 + y 2 x ′ = x ∗ ( 1 + k 1 ∗ r 2 + k 2 ∗ r 4 ) + 2 ∗ p 1 ∗ x ∗ y + p 2 ∗ ( r 2 + 2 ∗ x 2 ) y ′ = y ∗ ( 1 + k 1 ∗ r 2 + k 2 ∗ r 4 ) + 2 ∗ p 2 ∗ x ∗ y + p 1 ∗ ( r 2 + 2 ∗ y 2 ) r = \sqrt{x^2+y^2}\\ x' = x*(1+k_1*r^2+k_2*r^4)+2*p_1*x*y+p_2*(r^2+2*x^2)\\ y' = y*(1+k_1*r^2+k_2*r^4)+2*p_2*x*y+p_1*(r^2+2*y^2)\\ r=x2+y2 ​x′=x∗(1+k1​∗r2+k2​∗r4)+2∗p1​∗x∗y+p2​∗(r2+2∗x2)y′=y∗(1+k1​∗r2+k2​∗r4)+2∗p2​∗x∗y+p1​∗(r2+2∗y2)

3. 去畸变操作结束后，将相机坐标系重新转换到图像像素坐标系
   u ′ = x ′ ∗ f x + c x v ′ = y ′ ∗ f y + c y u'=x'*f_x+c_x\\ v'=y'*f_y+c_y u′=x′∗fx​+cx​v′=y′∗fy​+cy​

4. 用源图像的像素值对新图像的像素点进行插值

代码修改部分

// u(x) 列 v(y) 行
double u_distorted = 0, v_distorted = 0;            
// TODO 按照公式，计算点(u,v)对应到畸变图像中的坐标
// start your code here

// 把像素坐标系的点投影到归一化平面
double x = (u-cx)/fx, y = (v-cy)/fy; 

// 计算图像点坐标到光心的距离；
double r = sqrt(x*x+y*y);

// 计算投影点畸变后的点
double x_distorted = x*(1+k1*r+k2*r*r)+2*p1*x*y+p2*(r+2*x*x); 
double y_distorted = y*(1+k1*r+k2*r*r)+2*p2*x*y+p1*(r+2*y*y); 

// 把畸变后的点投影回去
u_distorted = x_distorted*fx+cx;
v_distorted = y_distorted*fy+cy;
// end your code here

运行结果截图

二 双目视差的使用​

双目相机的一大好处是可以通过左右目的视差来恢复深度。课程中我们介绍了由视差计算深度的过程。本题，你需要根据视差计算深度，进而生成点云数据。本题的数据来自Kitti 数据集[2]。
​ Kitti 中的相机部分使用了一个双目模型。双目采集到左图和右图，然后我们可以通过左右视图恢复出深度。经典双目恢复深度的算法有BM(Block Matching), SGBM(Semi-Global Block Matching)[3, 4] 等，
但本题不探讨立体视觉内容（那是一个大问题）。我们假设双目计算的视差已经给定，请你根据双目模型，画出图像对应的点云，并显示到Pangolin 中。
​ 本题给定的左右图见code/left.png 和code/right.png，视差图亦给定，见code/right.png。双目的参数如下：
f x = 718.856 ; f y = 718.856 ; c x = 607.1928 ; c y = 185.2157 : f_x = 718.856; f_y = 718.856; c_x = 607.1928; c_y = 185.2157: fx​=718.856;fy​=718.856;cx​=607.1928;cy​=185.2157:
且双目左右间距（即基线）为：
d = 0.573 m : d = 0.573 m: d=0.573m:
请根据以上参数，计算相机数据对应的点云，并显示到Pangolin 中。程序请参考code/disparity.cpp 文件。

答：课本中的双目相机模型如下：

[外链图片转存失败,源站可能有防盗链机制,建议将图片保存下来直接上传(img-qQqTRudg-1592674995380)(曾是少年-第四章作业.assets/image-20200605134649792.png)]

深度计算公式为：
d e p t h = f ∗ b d depth = \frac{f*b}{d} depth=df∗b​
在程序中，视差disp由深度图提供(uchar类型)。，f焦距由 f x f_x fx​给出，b是基线距离（程序中由d表示，可能会有一点混淆）。

课本中提到。虽然由视差计算深度的公式很简洁，但视差d 本身的计算却比较困难。本程序中已经提供了视差图因此很容易计算得到深度。

注意事项：

• 计算点的时候需要把像素点先转换到相机坐标系。
• 程序中基线距离的表示符号为d
• 视差图中数据类型为uchar
• 平时中焦距 f f f与 f x f_x fx​差不多

点云计算代码

// TODO 根据双目模型计算点云
// 如果你的机器慢，请把后面的v++和u++改成v+=2, u+=2
for (int v = 0; v < left.rows; v++)
    for (int u = 0; u < left.cols; u++) {

        Vector4d point(0, 0, 0, left.at<uchar>(v, u) / 255.0); // 前三维为xyz,第四维为颜色
        // start your code here (~6 lines)
        // 根据双目模型计算 point 的位置
        double x = (u-cx)/fx;
        double y = (v-cy)/fy;
        float disp = disparity.at<uchar>(v,u); //视差
        double depth = fx*d/(disp);//  d是基线
        point[0] = x*depth;
        point[1] = y*depth;
        point[2] = 1*depth;
        pointcloud.push_back(point);
        // end your code here
    }

生成的点云截图如下所示：

[外链图片转存失败,源站可能有防盗链机制,建议将图片保存下来直接上传(img-JX6J9Rrr-1592674995382)(image/点云结果.png)]

三 矩阵运算微分

在优化中经常会遇到矩阵微分的问题。例如，当自变量为向量x，求标量函数u(x) 对x 的导数时，即为矩阵微分。通常线性代数教材不会深入探讨此事，这往往是矩阵论的内容。我在ppt/目录下为你准备了一份清华研究生课的矩阵论课件（仅矩阵微分部分）。阅读此ppt，回答下列问题：
设变量为 x ∈ R N x \in R^N x∈RN，(x是列向量) 那么：

1. 矩阵 A ∈ R N × N A \in R^{N\times N} A∈RN×N，那么d(Ax)/dx 是什么？

答： x x x是 n × 1 n\times1 n×1列向量

令矩阵 A = [ a 1 , a 2 , . . . , a n ] A = [a_1,a_2,...,a_n] A=[a1​,a2​,...,an​] , A = [ a 1 ′ ; a 2 ′ ; . . . ; a n ′ ] A = [a_1';a_2';...;a_n'] A=[a1′​;a2′​;...;an′​]。

∂ A x ∂ x = [ ∂ A x 1 ∂ x 1 ∂ A x 2 ∂ x 1 . . . ∂ A x n ∂ x 1 ∂ A x 1 ∂ x 2 ∂ A x 2 ∂ x 2 . . . ∂ A x n ∂ x 2 . . . . . . . . . . . . ∂ A x 1 ∂ x n ∂ A x 2 ∂ x n . . . ∂ A x n ∂ x n ] \begin{aligned} \frac{\partial{{Ax}}}{\partial x} &= \left[ \begin{array}{ccc} \frac{\partial{{Ax}_1}}{\partial x_1}& \frac{\partial{Ax}_2}{\partial x_1}& ...& \frac{\partial{Ax}_n}{\partial x_1}\\ \frac{\partial{{Ax}_1}}{\partial x_2}& \frac{\partial{Ax}_2}{\partial x_2}& ...& \frac{\partial{Ax}_n}{\partial x_2}\\ ... & ... &...&...\\ \frac{\partial{{Ax}_1}}{\partial x_n}& \frac{\partial{Ax}_2}{\partial x_n}& ...& \frac{\partial{Ax}_n}{\partial x_n}\\ \end{array} \right] \end{aligned} ∂x∂Ax​​=⎣⎢⎢⎡​∂x1​∂Ax1​​∂x2​∂Ax1​​...∂xn​∂Ax1​​​∂x1​∂Ax2​​∂x2​∂Ax2​​...∂xn​∂Ax2​​​............​∂x1​∂Axn​​∂x2​∂Axn​​...∂xn​∂Axn​​​⎦⎥⎥⎤​​
先对x的第i个分量求导：
∂ A x i ∂ x k = ∂ a i x ∂ x k = a i k \begin{aligned} \frac{\partial{Ax}_i}{\partial x_k} &= \frac{\partial{a_ix}}{\partial x_k} =a_{ik} \end{aligned} ∂xk​∂Axi​​​=∂xk​∂ai​x​=aik​​
导入前式有：
∂ A x ∂ x = [ a 11 a 21 . . . a n 1 a 12 a 22 . . . a n 2 . . . . . . . . . . . . a 1 n a 2 n . . . a n n ] = A T \begin{aligned} \frac{\partial{{Ax}}}{\partial x} &= \left[ \begin{array}{ccc} a_{11} & a_{21} & ...& a_{n1}\\ a_{12} & a_{22} & ... & a_{n2}\\ ... & ... &...&...\\ a_{1n} & a_{2n} & ...& a_{nn}\\ \end{array} \right] \end{aligned} = A^T ∂x∂Ax​​=⎣⎢⎢⎡​a11​a12​...a1n​​a21​a22​...a2n​​............​an1​an2​...ann​​⎦⎥⎥⎤​​=AT

2. 矩阵 A ∈ R N × N A \in R^{N\times N} A∈RN×N，那么 d ( x T A x ) / d x d(x^TAx)/dx d(xTAx)/dx 是什么？

答：
∂ x T A x ∂ x = [ ∂ x T A x ∂ x 1 ∂ x T A x ∂ x 2 . . . ∂ x T A x ∂ x n ] \begin{aligned} \frac{\partial{x^TAx}}{\partial x} &= \left[ \begin{array}{ccc} \frac{\partial{x^TAx}}{\partial x_1}& \frac{\partial{x^TAx}}{\partial x_2}& ...& \frac{\partial{x^TAx}}{\partial x_n} \end{array} \right] \end{aligned} ∂x∂xTAx​​=[∂x1​∂xTAx​​∂x2​∂xTAx​​...​∂xn​∂xTAx​​]​
先对x的第k个分量求导，结果如下：
∂ x T A x ∂ x k = ∂ ∑ i = 1 n ∑ j = 1 n x i A i j x j ∂ x k = ∑ i = 1 n A i k x i + ∑ j = 1 n A k j x j = a k T x + a k ′ x \begin{aligned} \frac{\partial{x^TAx}}{\partial x_k} &= \frac{\partial{\sum^n_{i=1}\sum_{j=1}^nx_{i}A_{ij}x_j}}{\partial x_k}\\ &=\sum^n_{i=1} A_{ik}x_i+\sum^n_{j=1}A_{kj}x_j\\ &=a^T_kx+a'_kx \end{aligned} ∂xk​∂xTAx​​=∂xk​∂∑i=1n​∑j=1n​xi​Aij​xj​​=i=1∑n​Aik​xi​+j=1∑n​Akj​xj​=akT​x+ak′​x​
可以看出第一部分是矩阵A的第k列转置后和x相乘得到，第二部分是矩阵A的第k行和x相乘得到，排列好就是:
∂ x T A x ∂ x = A T x + A x \frac{\partial{x ^ T Ax}}{\partial x} = A^Tx+Ax ∂x∂xTAx​=ATx+Ax

3. 证明： x T A x = t r ( A x x T ) x^TAx = tr(Axx^T) xTAx=tr(AxxT)

证明：

设a,b都是n维列向量，显然有
a b T = [ a 1 b 1 a 1 b 2 . . . a 1 b n a 2 b 1 a 2 b 2 . . . a 2 b n . . . . . . . . . . . . a n b 1 a n b 2 . . . a n b n ] ab^T= \left[ \begin{array}{ccc} a_1b_1&a_1b_2&...&a_1b_n\\ a_2b_1&a_2b_2&...&a_2b_n\\ ...&...&...&...\\ a_nb_1&a_nb_2&...&a_nb_n \end{array} \right] abT=⎣⎢⎢⎡​a1​b1​a2​b1​...an​b1​​a1​b2​a2​b2​...an​b2​​............​a1​bn​a2​bn​...an​bn​​⎦⎥⎥⎤​

b T a = ∑ i = 1 n a i b i b^Ta=\sum^{n}_{i=1}a_ib_i bTa=i=1∑n​ai​bi​

显然，可以得到：
t r ( a b T ) = b T a tr(ab^T)=b^Ta tr(abT)=bTa
令 a = A x a=Ax a=Ax , b = x b=x b=x 可得
t r ( A x x T ) = t r ( ( A x ) x T ) = x T A x tr(Axx^T)=tr((Ax)x^T)=x^TAx tr(AxxT)=tr((Ax)xT)=xTAx
证毕

附加参考：

四 高斯牛顿法的曲线拟合实验

我们在课上演示了用Ceres 和g2o 进行曲线拟合的实验，可以看到优化框架给我们带来了诸多便利。
本题中你需要自己实现一遍高斯牛顿的迭代过程，求解曲线的参数。我们将原题复述如下。设有曲线满足以下方程：
y = exp ⁡ ( a x 2 + b x + c ) + w . y = \exp(ax^2 + bx + c) + w. y=exp(ax2+bx+c)+w.
其中 a , b , c a, b, c a,b,c 为曲线参数，w为噪声。现有N个数据点 ( x , y ) (x,y) (x,y)，希望通过此N个点来拟合 a , b , c a, b, c a,b,c。实验中取 N = 100 N = 100 N=100。
那么，定义误差为 e i = y i − exp ⁡ ( a x i 2 + b x i + c ) e_i = y_i - \exp(ax^2_i+bx_i + c) ei​=yi​−exp(axi2​+bxi​+c)，于是 ( a , b , c ) (a, b,c) (a,b,c) 的最优解可通过解以下最小二乘获得：
min ⁡ a , b , c 1 2 ∑ i = 1 N ∣ ∣ y i exp ⁡ ( a x i 2 + b x i + c ) ∣ ∣ 2 \min_{a,b,c}\frac{1}{2}\sum^{N}_{i=1}||y_i\exp(ax_i^2+bx_i+c)||^2 a,b,cmin​21​i=1∑N​∣∣yi​exp(axi2​+bxi​+c)∣∣2
现在请你书写Gauss-Newton 的程序以解决此问题。程序框架见code/gaussnewton.cpp，请填写程序内容以完成作业。作为验证，按照此程序的设定，估计得到的a; b; c 应为： a = 0.890912 ; b = 2.1719 ; c = 0.943629 , a = 0.890912; b = 2.1719; c = 0.943629, a=0.890912;b=2.1719;c=0.943629,
这和书中的结果是吻合的。

答：先回顾高斯牛顿法求解最小二乘问题的步骤：
Δ x ∗ = arg ⁡ min ⁡ Δ x 1 2 ∣ ∣ f ( x ) + J ( x ) T Δ x ∣ ∣ 2 \Delta x^{*} = \arg \min_{\Delta x}\frac{1}{2}||f(x)+J(x)^T\Delta x||^2 Δx∗=argΔxmin​21​∣∣f(x)+J(x)TΔx∣∣2

1. 给定初始值 x 0 x_0 x0​。
2. 对于第k 次迭代，求出当前的雅可比矩阵 J ( x k ) J(x_k) J(xk​) 和误差 f ( x k ) f(x_k) f(xk​)。
3. 求解增量方程： H Δ x k = g HΔx_k = g HΔxk​=g。
4. 若 Δ x k Δx_k Δxk​ 足够小，则停止。否则，令 x k + 1 = x k + Δ x k x_{k+1} = x_k + Δx_k xk+1​=xk​+Δxk​，返回第2 步。

可以按照以上步骤来修改代码

1. 设置初始值

double ae = 2.0, be = -1.0, ce = 5.0;

2. 计算雅可比矩阵 J ( x k ) J(x_k) J(xk​) 和误差 f ( x k ) f(x_k) f(xk​)。

计算误差 e r r o r = f ( x i ) − f e ( x i ) error = f(x_i)-f_e(x_i) error=f(xi​)−fe​(xi​)

error = yi - exp(ae * xi * xi + be * xi + ce);

计算雅可比矩阵$J = \frac{\partial error} {\partial x} $

Vector3d J; // 雅可比矩阵
J[0] = - exp(ae * xi * xi + be * xi + ce)* xi * xi;  // de/da
J[1] = - exp(ae * xi * xi + be * xi + ce)* xi;  // de/db
J[2] = - exp(ae * xi * xi + be * xi + ce);  // de/dc

3. 求解增量方程

计算增量矩阵H

H += J * J.transpose(); // GN近似的H

计算g

b += -error * J;

用EIgen中的ldlt求解 H Δ x = b H\Delta x =b HΔx=b。

Vector3d dx;
dx = H.ldlt().solve(b);

4. 若 Δ x k Δx_k Δxk​ 足够小，则停止。否则，令 x k + 1 = x k + Δ x k x_{k+1} = x_k + Δx_k xk+1​=xk​+Δxk​，返回第2 步。

if (iter > 0 && cost > lastCost) {
    // 误差增长了，说明近似的不够好
    cout << "cost: " << cost << ", last cost: " << lastCost << endl;
    break;
}

至此，代码修改完毕。

运行结果：

/home/guoben/Project/SLAM-homework/ch4/GaussNewton/bin/GN
total cost: 3.19575e+06
total cost: 376785
total cost: 35673.6
total cost: 2195.01
total cost: 174.853
total cost: 102.78
total cost: 101.937
total cost: 101.937
total cost: 101.937
total cost: 101.937
total cost: 101.937
total cost: 101.937
total cost: 101.937
cost: 101.937, last cost: 101.937
estimated abc = 0.890912, 2.1719, 0.943629

Process finished with exit code 0

运行截图

附加题 五* 批量最大似然估计

考虑离散时间系统：
x k = x k − 1 + v k + w k ; w ∼ N ( 0 ; Q ) y k = x k + n k ; n k ∼ N ( 0 ; R ) x_k = x_{k-1} + v_k + w_k; w\sim N (0;Q)\\ y_k = x_k + n_k; n_k \sim N (0;R) xk​=xk−1​+vk​+wk​;w∼N(0;Q)yk​=xk​+nk​;nk​∼N(0;R)
这可以表达一辆沿x 轴前进或后退的汽车。第一个公式为运动方程， v k v_k vk​ 为输入， w k w_k wk​ 为噪声；第二个公式为观测方程， y k y_k yk​ 为路标点。取时间 k = 1 , . . . , 3 k = 1,...,3 k=1,...,3，现希望根据已有的 v , y v,y v,y 进行状态估计。设初始状态 x 0 x_0 x0​ 已知。
请根据本题题设，推导批量（batch）最大似然估计。首先，令批量状态变量为

x = [ x 0 , x 1 , x 2 , x 3 ] T x = [x_0, x_1, x_2, x_3]^T x=[x0​,x1​,x2​,x3​]T，令批量观测为 z = [ v 1 , v 2 , v 3 , y 1 , y 2 , y 3 ] T z = [v_1, v_2, v_3, y_1, y_2, y_3]^T z=[v1​,v2​,v3​,y1​,y2​,y3​]T，那么：

1. 可以定义矩阵 H，使得批量误差为 e = z − H x e = z - Hx e=z−Hx。请给出此处H的具体形式。

答：该线性系统很简单，很容易的写成以下形式
v k = x k − x k − 1 + w k y k = x k + n k v_k = x_k-x_{k-1} + w_k\\ y_k= x_k + n_k\\ vk​=xk​−xk−1​+wk​yk​=xk​+nk​
而 z − H x = e ∼ N ( 0 , Σ ) z-Hx=e\sim N(0,\Sigma) z−Hx=e∼N(0,Σ), 向量化上式可以得到：
H = [ − 1 1 0 0 0 − 1 1 0 0 0 − 1 1 0 1 0 0 0 0 1 0 0 0 0 1 ] H= \left[ \begin{array}{ccc} -1& 1& 0& 0\\ 0 &-1& 1& 0\\ 0 & 0&-1& 1\\ 0&1&0&0\\ 0&0&1&0\\ 0&0&0&1\\ \end{array} \right] H=⎣⎢⎢⎢⎢⎢⎢⎡​−100000​1−10100​01−1010​001001​⎦⎥⎥⎥⎥⎥⎥⎤​

2. 据上问，最大似然估计可转换为最小二乘问题, 请给出此问题下信息矩阵W 的具体取值。

x ∗ = arg ⁡ min ⁡ 1 2 ( z − H x ) T W − 1 ( z − H x ) x^{*} = \arg \min \frac{1}{2}(z - Hx)^TW^{-1}(z-Hx) x∗=argmin21​(z−Hx)TW−1(z−Hx)

其中W 为此问题的信息矩阵，可以从最大似然的概率定义给出。

答： W = d i a g ( Q , R ) W=diag(Q,R) W=diag(Q,R)
x ∗ = arg ⁡ max ⁡ P ( x ∣ z ) = arg ⁡ max ⁡ P ( z ∣ x ) = ∏ k = 1 3 P ( v k ∣ x k − 1 , x k ) ∏ k = 1 3 P ( y k ∣ x k ) \begin{aligned} x^{*} &= \arg \max P(x|z) = \arg \max P(z|x)\\ &=\prod^{3}_{k=1}P(v_k|x_{k-1},x_k)\prod^{3}_{k=1}P(y_k|x_k) \end{aligned} x∗​=argmaxP(x∣z)=argmaxP(z∣x)=k=1∏3​P(vk​∣xk−1​,xk​)k=1∏3​P(yk​∣xk​)​
其中 P ( v k ∣ x k − 1 , x k ) = N ( x k − x k − 1 , Q ) P(v_k|x_{k-1},x_k)=N(x_k-x_{k-1},Q) P(vk​∣xk−1​,xk​)=N(xk​−xk−1​,Q)，

P ( y k ∣ x k ) = N ( x k , R ) P(y_k|x_k) = N(x_k,R) P(yk​∣xk​)=N(xk​,R)。

误差变量如下：
e v , k = x k − x k − 1 − v k , e z , k = y k − x k e_{v,k}=x_k-x_{k-1}-v_k, e_{z,k}=y_k-x_k ev,k​=xk​−xk−1​−vk​,ez,k​=yk​−xk​
对概率取对数，可以把最小二乘的目标函数化为如下形式：
min ⁡ ∑ k = 1 3 e v , k T Q − 1 e v , k + ∑ k = 1 3 e y , k T R − 1 e y , k \min\sum^3_{k=1} e^{T}_{v,k}Q^{-1}e_{v,k}+\sum^3_{k=1}e^T_{y,k}R^{-1}e_{y,k} mink=1∑3​ev,kT​Q−1ev,k​+k=1∑3​ey,kT​R−1ey,k​
因此 W = d i a g ( Q , Q , Q , R , R , R ) W=diag(Q,Q,Q,R,R,R) W=diag(Q,Q,Q,R,R,R); 即
W = [ Q 0 0 0 0 0 0 Q 0 0 0 0 0 0 Q 0 0 0 0 0 0 R 0 0 0 0 0 0 R 0 0 0 0 0 0 R ] W = \left[ \begin{array}{ccc} Q & 0 & 0 & 0 & 0 & 0\\ 0 & Q & 0 & 0 & 0 & 0\\ 0 & 0 & Q & 0 & 0 & 0\\ 0 & 0 & 0 & R & 0 & 0\\ 0 & 0 & 0 & 0 & R & 0\\ 0 & 0 & 0 & 0 & 0 & R\\ \end{array} \right] W=⎣⎢⎢⎢⎢⎢⎢⎡​Q00000​0Q0000​00Q000​000R00​0000R0​00000R​⎦⎥⎥⎥⎥⎥⎥⎤​
此时，最小二乘问题可以写为：
x ∗ = arg ⁡ min ⁡ e T W − 1 e x^{*} =\arg \min e^T W^{-1} e x∗=argmineTW−1e

3. 假设所有噪声相互无关，该问题存在唯一的解吗？若有，唯一解是什么？若没有，说明理由。

答: 当噪声相互无关的时候，该问题存在唯一解。

因为 H x = z Hx=z Hx=z 这个式子中H是6*4矩阵，方程个数大于未知量个数的方程组，是一个超定矩阵。而系数矩阵超定时，最小二乘问题可以得到唯一解。
唯一最小二乘解如下：
x = ( H T H ) − 1 H T z x=(H^TH)^{-1}H^Tz x=(HTH)−1HTz

助教点评：假设所有噪声相互无关，那么H的秩是等于4的，所以问题存在唯一解，那根据本题定义，我们可以将目标函数写成图中14式所示，因为JX刚好是一个抛物面，我们能解析的找到它的最小值，这只需要让目标函数相对于自变量的偏导数为零即可得到啊，如图中所示，我们可以得到最后的一个X最优解。