TOA的最小二乘算法公式推导

TOA的最小二乘算法公式推导如下：

1. 算法原理：
- TOA定位中，广义量测函数表示为r = h(x) + v，其中r是测量距离，h(x)是距离的估计值，v是测量误差。
- 最小二乘法的目标是最小化测量误差的平方和，即min(v^T * v)。

2. 迭代最小二乘：
- 首先，对广义量测函数进行泰勒级数展开，得到一阶近似：r ≈ h(x) + H(x)(x - x0) + v，其中H(x)是雅可比矩阵，x0是初始估计值。
- 然后，将r代入最小化目标函数，对目标函数进行求导并令导数为0，得到最小二乘估计值的迭代公式：x = x0 - (H^T * H)^(-1) * H^T * v。

3. 最大似然估计—高斯牛顿法：
- 高斯牛顿法是一种迭代算法，通过不断线性化广义量测函数来逼近最优解。
- 首先，对广义量测函数进行泰勒级数展开，得到一阶近似：r ≈ h(x) + H(x)(x - x0) + v，其中H(x)是雅可比矩阵，x0是初始估计值。
- 然后，将r代入最大似然估计目标函数，对目标函数进行求导并令导数为0，得到高斯牛顿法的迭代公式：x = x0 -1) * v，其中R是测量误差的协方差矩阵。