## 5.5.1

A=1,B=1,C=1

增广系统可观测

## 5.5.2

其中

增广系统可观测

## 5.5.3

根据式(5.3.7)
迭代次数

## 5.5.4

从上面第一章图可以看出，当前鲁棒函数在

从第二章图，代价函数的导数绝对值函数可以看出，在无外点干扰的情况下，平方代价函数收敛更快

在

可以看到，该算法在外点偏大区域与German-McClure代价函数具有相当的抑制作用。

总结：在非外点区域更快迭代收敛，在外点偏大区域具备相当的异常抑制作用，既鲁棒又快

源码和pdf地址：

https://github.com/mingtiancai/estimation_for_robotics_answer/tree/master/cs​github.com