Nonlinear Least Squares
\[
\mathbf{x^{*}} = argmin| \mathbf{f(x)} |^2_{\mathbf{\Omega}}
\]
其中\(\mathbf{f(x)}\)满足均值为0,方差为\(\mathbf{R} = \mathbf{\Omega}^{-1}\)的正态分布。
设初始值为\(\mathbf{x_0}\),求解\(\delta\mathbf{x}\),使得\(argmin|\mathbf{f(x_0 + \delta x)}|^{2}_{\mathbf{\Omega}}\)最小。
\[
\begin{aligned}
r &=\boldsymbol{f}\left(\boldsymbol{x}_{0}+\delta \boldsymbol{x}\right)^{T} \boldsymbol{\Omega} \boldsymbol{f}\left(\boldsymbol{x}_{0}+\delta \boldsymbol{x}\right) \\
&=\left(\boldsymbol{f}\left(\boldsymbol{x}_{0}\right)+\boldsymbol{J} \delta \boldsymbol{x}\right)^{T} \boldsymbol{\Omega}\left(\boldsymbol{f}\left(\boldsymbol{x}_{0}\right)+\boldsymbol{J} \delta \boldsymbol{x}\right) \\
&=\boldsymbol{f}\left(\boldsymbol{x}_{0}\right)^{T} \boldsymbol{\Omega} \boldsymbol{f}\left(\boldsymbol{x}_{0}\right)+2 \delta \boldsymbol{x}^{T} \boldsymbol{J}^{T} \boldsymbol{\Omega} \boldsymbol{f}\left(\boldsymbol{x}_{0}\right)+\delta \boldsymbol{x}^{T} \boldsymbol{J}^{T} \boldsymbol{\Omega} \boldsymbol{J} \delta \boldsymbol{x}
\end{aligned}
\]
当\(r\)对导数为0时，取得最小值：
\[
\frac{\partial r}{\partial\delta \mathbf{x}} = 2 \mathbf{J}^T\mathbf{\Omega}\mathbf{J}\delta \mathbf{x} + 2\mathbf{J}^T\mathbf{\Omega}\mathbf{f}(\mathbf{x}_0) = \mathbf{0}
\]
得：
\[
\mathbf{J}^t\mathbf{\Omega}\mathbf{J}\delta \mathbf{x} = -\mathbf{J}^T\mathbf{\Omega}\mathbf{f}(\mathbf{x}_0)
\]
状态更新：
\[
\mathbf{x}_1 = \mathbf{x}_0 + \delta \mathbf{x}
\]
为了减少outlier对求解的影响，可以加入Huber Loss Function实现鲁棒性估计。
\[
\begin{aligned}
\boldsymbol{x}^{*} &=\operatorname{argmin} \sum_{i} \rho\left(\left|\boldsymbol{f}_{i}(\boldsymbol{x})\right|_{\boldsymbol{\Omega}_{i}}^{2}\right) \\
&=\operatorname{argmin} \sum_{i} \rho\left(r_{i}\right)
\end{aligned}
\]
其中Huber Loss如下：
\[
\rho(r)= \begin{cases}r & r^{1 / 2}<\gamma \\ 2 \gamma \cdot r^{1 / 2}-\gamma^{2} & r^{1 / 2} \geq \gamma\end{cases}
\]
\(\gamma\)一般取卡放分布95%的值。
当\(\rho(r)\)对\(\delta \mathbf{x}\)的导数为0取得最小值：
\[
\begin{aligned}
\partial \rho(r) / \partial \delta \boldsymbol{x} &=\partial \rho(r) / \partial r . \partial r / \partial \delta \boldsymbol{x} \\
&=2 \boldsymbol{J}_{\rho} \boldsymbol{J}^{T} \mathbf{\Omega} \boldsymbol{J} \delta \boldsymbol{x}+2 \boldsymbol{J}_{\rho} \boldsymbol{J}^{T} \boldsymbol{\Omega} \boldsymbol{f}\left(\boldsymbol{x}_{0}\right) \\
&=2 \sum_{i} \boldsymbol{J}_{i}^{T} \boldsymbol{J}_{\rho_{i}} \mathbf{\Omega} \boldsymbol{J}_{i} \delta \boldsymbol{x}+2 \boldsymbol{J}_{i}^{T} \boldsymbol{J}_{\rho_{i}} \boldsymbol{\Omega}_{i} \boldsymbol{f}_{i}\left(\boldsymbol{x}_{0}\right) \\
&=\mathbf{0}
\end{aligned}
\]
其中：
\[
\boldsymbol{J}_{\rho}= \begin{cases}1 & r^{1 / 2}<\gamma \\ \frac{\gamma}{r^{1 / 2}} & r^{1 / 2} \geq \gamma\end{cases}
\]
迭代公式：
\[
\sum_{i} \boldsymbol{J}_{i}^{T} \boldsymbol{J}_{\rho_{i}} \boldsymbol{\Omega} \boldsymbol{J}_{i} \delta \boldsymbol{x}=-\boldsymbol{J}_{i}^{T} \boldsymbol{J}_{\rho_{i}} \boldsymbol{\Omega}_{i} \boldsymbol{f}_{i}\left(\boldsymbol{x}_{0}\right)
\]
可以看到Huber Loss Function自动降低outlier权重。使得估计鲁棒性提高。
Extended Kalman Filter
在EKF更新过程中：
\[
\begin{aligned}
\mathbf{e} &= \mathrm{z}_{\mathrm{k}+1}-\hat{\mathbf{z}}_{\mathrm{k}+1 \mid \mathrm{k}} \\
K_{k+1}&=P_{k+1 \mid k} H_{k+1}^{\mathrm{T}}\left(H_{k+1} P_{k+1 \mid k} H_{k+1}^{\mathrm{T}}+R_{k+1}\right)^{-1} \\
\hat{\boldsymbol{x}}_{k+1 \mid \mathrm{k}+1}&=\hat{\boldsymbol{x}}_{k+1 \mid \mathrm{k}}+K_{k+1}\left(e\right) \\
P_{k+1 \mid k+1} &=\left(I-K_{k+1} H_{k+1}\right) P_{k+1 |\mathrm{k}}
\end{aligned}
\]
可以加入huber核函数，降低outlier的权重，增大协方差。
\[
\begin{aligned}
K_{k+1}&=P_{k+1 \mid k} H_{k+1}^{\mathrm{T}}\left(H_{k+1} P_{k+1 \mid k} H_{k+1}^{\mathrm{T}}+R_{k+1} * J_{\rho}^{-1} \right)^{-1}
\end{aligned}
\]
其中：
\[
r = \mathbf{e}^T(H_{k+1} P_{k+1 \mid k} H_{k+1}^{\mathrm{T}}+R_{k+1}) \mathbf{e}
\]
\[
J_{\rho} = \partial{\rho(r)}/\partial{r}
\]
Reference
[1] https://zhuanlan.zhihu.com/p/524339818
[2] Comparison of Several Nonlinear Filters for a Benchmark Tracking Problem