后来发现CDKF是个已有的东西。
https://tech.hqew.com/fangan_1581196
(资料图片)
转载自：https://mp.weixin.qq.com/s/tofnQkm4ZAoi2tfwlPS5qg
TSLAM9--------中心差分卡尔曼滤波（cdkf）
原创 凌拓智能TBUS TBUS社区 今天
新一年的算法更新季又来了，这一次tbus的优化侧重点任然放在状态估计方面。
去年推出了tiny_ekf主要侧重点在飞的稳不稳方面，而今年由于业务量的增加，估计精度的需求也凸显出来。
我们直接进入主题：
一.Tslam9功能性更新-----适配镭神n301远距离激光雷达：
不得不说，镭神n301比思岚系列雷达精度高太多太多，我们的结构都常常跟我说，我们把标配改成雷神算了，思岚太垃圾！-_-！！但是L还是坚持，算了N301还是太重了，5倍重量换精度，其实不值得，毕竟是飞机，还是列为可选件吧！-_-！！
（请相信我不是他们的托，其实人家也看不起我们这种托）
下面给大家看一些他们官网上看不到的参数，都是tbus实测得出的：
1. 精度：近处3cm，远处按距离和反射材质下降，40m处最多不会超过20cm，相比之下思岚40m处有可能会在30-40cm之间。
2. 测量范围：白色100m，灰色70m，纯黑色找不到环境，但是估计达到30m没有问题。
3. 耐CAO性：实测建图24小时不发散，证明硬件工作正常，固件没有BUG。
4. 真实角度分辨率：0.36度，官方说的0.012度的分辨率实际上是线性插值得到的，其实有编码器约束的角度分辨率任然是0.36度。
5. 噪声分布：正太分布，峰高稍微有一点低，有时会畸变
6. 延迟控制：由于是网口传的数据，网口太快了，就算代码写的再垃圾也基本1ms以下的延迟且各点均匀发送，所以看不出什么。但至少没有逻辑BUG。
总的来说，精度高距离远，由于重量大，用在大场景大飞机上是特别适合的。
二.Tslam9算法更新----新的状态估计器，中心差分卡尔曼滤波（cdkf）
客户给L出了一个题：请把脚架大小30*30的无人机，通过二维码引导降落停在一个45*45平台上。L一口答应！这种找角点的垃圾游戏，对L来说犹如吃饭喝水。
几天下来，代码哗哗写完，自信满满进入测试阶段，接下来的惨状，我就不多说了，脸打的啪啪响。
经过曲折离奇的调试，最终发现，其实问题出在状态估计上，在面临这种精确停靠的地方，需要做精确控制，由于状态给的不够准确，控制都很紊乱，那肯定停不好咯。
具体出现了以下几个问题：
1. 无人机处于悬停状态一直调节，不落下
2. 如果把容差范围设大，下落倒是可以很快了，但是误差会特别大，10次甚至有2-3次会超出范围
3. 快接近目标水平坐标以后，微量的调节干脆直接就不响应
以上三个问题分别对应于：速度估计不准确，位置估计不准确，姿态估计不准确
L想了很多迂回方案，都不是很恰当，最终决定还是刚正面才是治本之道。
于是：
（cdkf位置）
（ekf位置）
（cdkf速度）
（ekf速度）
以上两组数据测试条件均为：无人机在地面上解锁后定点模式怠速（为啥要解锁？因为不解锁不会打印日志。为啥要定点怠速，因为自稳桨不转10S后会自动加锁。）
我们用肉眼观测：
cdkf速度标准差：2cm ekf速度标准差：3.5cm
cdkf位置标准差：1cm ekf位置标准差：2cm
性能有2倍提升，然而最关键的是，cdkf如实的描述了实际情况，而ekf并没有！！！
我们已经说过测试环境是地面上解锁后的定点怠速，那么可以想象飞机一定会有2-3mm的周期性往返运动，cdkf试图努力的描述这个事实，而ekf并没有。（由于tslam水平位置测量方差一般设置1m，所以显得振幅大于实际值，若如实填写方差，效果会更加接近真实）
综上所述，cdkf相较于ekf，精度更高，反应更灵敏，非常适合做精准控制！
（感谢后面羽毛球小妹妹友情出演，证明时间轴）
————————————————华丽的分界线—————————————————
上面广告打完了，老板们请出门右转，已为你们提供总统套房。
想掉头发的工程师请跟着L继续下矿挖煤。
1.其实一开始仅仅是想搞ukf啊！！！
如果ekf不够用怎么办？上ukf撒，精度高，反应灵敏，计算量小。。。一百度全TM在说优点，尤其可耻的百度学术上的论文，拿着1维状态，仿照公式写个demo，matlab做个验证。三板斧一下就开启吹牛模式，最关键的是数学渣L信了！就算曾经被论文骗过无数次的L这次也信了，因为吹的太真实的，仿佛临死之前看到了乌托邦！
几天编码下来进入测试阶段，我就发现错了，仿佛全世界的恶意都在向我招手，对的，我陷入了无穷无尽的-nan(ind)。
一般情况下百度会告诉你alpha=0.001 beta=2 kappa=0
Lambda=alpha^2(n+kappa)-n ----(n=状态维度)
而实际L让他稳定下来的那组参数是：
alpha=1 beta=0 kappa=3
此时lambda=kappa Wm0=Wc0
经过长时间飞行测试发现，WC，这组参数确实非常稳定，无可挑剔，但是L还是心里发慌，完全没有经过数学验证，瞎调参数，这种东西别说发布，就连做项目都感觉危险。。。。。。直到我发现了这一篇帖子：
https://tech.hqew.com/fangan_1581196
看出问题了吗？WC！
当alpha=1 beta=0 n+kappa=h^2 n=维度 且h=sqrt(3)时：
ukf = cdkf高斯最优
好吧，数学渣就是数学渣，可能别人几秒想通的事情L绕了一个大圈圈。不过能搞定还是很幸喜的，现在有了数学支撑，那一定是很健壮的了，匿了匿了。。。
那么ukf到底是怎么回事呢？如果你google其实你会发现，ukf只适合处理1-3维变量的问题，大于3非常不稳定，但是由于C国写论文做验证的，一般都只拿1维变量做测试，所以。。。。。。实际工程中是不可能小于3维变量的，那种东西，也只能拿来写论文了- -。
2. cdkf一定比ekf好吗？NO！
ukf cdkf ckf。。。。。。这一类统称为：spkf(sigma point kalman filter)，都是通过采样点做状态转移，然后估计转移后协方差的方法来对抗非线性问题的。这一类方法无法避免的一个问题是：矩阵P的chol分解，就是对一个矩阵开根号。（其实可以想象成对一个数开根号）
那么必须要满足的条件是：这个数大于等于0，推广到矩阵就是矩阵必须是正定或者半正定。
但是由于数值舍入，数学模型舍入，传感器噪声特性，Q、R阵设置等问题，负定的概率是比较大的。（但是可以接受，通过一些trick可以搞定，下文会讲）
所以，在不需要高精度的场景下，依然是推荐ekf的，ekf本身对P阵是否正定是不敏感的，最多不过就是发散，绝对不会出现-nan(ind)，-nan(ind)对飞控的控制来说是一个灾难，如果ekf位置发散了几十米，我还可以用遥控器手动拉回来，但如果是数值错误，你就一点机会也没有了-_-！！
对于解决此问题的方法网上也比比皆是，浏览一下基本上都是说用svd分解代替chol分解，或者当chol出现问题的时候，用svd找临近半正定。
哎，其实这也是一个坑，我们的飞控400hz调度，每一次调度需要一次卡尔曼的predict，每一次只有2.5ms的时间，除开控制，mavlink通讯，log日志以及错误检测那些代码，留给卡尔曼的就算还有2ms，但是你叫我在一个arm核上用2ms做一个svd分解？太难了，太难了。。。
所以L还是决定，暴力一点，能用就行：如果chol出现问题，我们就直接把P阵非对角元全部清0，这样他一定是一个正定矩阵，而且跟原值差别不大。（其实有点大，只是不得已的方法里面的最小，至少方差部分信息是全部保留了。）
经验证，收敛情况下cdkf是不会负定P的，出现是因为，gps飘了、slam飘了，或者QR阵设置不合理，P阵初值不合理，状态初值不合理（限制有点多呀！），做了trick以后，就算GPS飘了、SLAM飘了，也不会出现-nan(ind)。
3. 网上说ukf和ekf运算效率是一个数量级的！NO!
实测19*19维状态变量的情况下，计算耗时：cdkf=8*ekf
并且随着维度的增加，比例还会增加，由于网上都是拿一维说事儿，所以当然差不多，甚至ukf比ekf还快。
所以，单片机的控，至少stm32f4系列是歇菜了，f7也许能战，h7肯定能战
4. 目前有比cdkf更好的状态估计方法吗？YES！
这是自然，这里有一张老外的图：
cdkf相对于ukf、3阶ckf表现效果更好，但是任然比不过5阶ckf（容积卡尔曼滤波）
2. 目前还有哪些优化空间呢？
总的来说评价状态估计有3个指标：
1. 稳定性
2. 精度
3. 鲁棒性
精度提升空间已经不太大了，据我所知，下来最多换ckf-5th，但任然是基于sigma point的估计意思不大。
稳定性方面的话，还有很大空间，tbus一直都是在用直接卡尔曼滤波，其实对于旋转的估计，间接卡尔曼滤波是航空航天一直都在使用的方式，可以搜索：eskf或者mekf。
优势在这里：
鲁棒方面的话基本没做，说白了就是自适应。这方面的话不太了解，但是看论文提升是挺大的，GPS故障之类的都能保持稳定，下次升级也许会尝试。
------------------------------------------------华丽的分界线-------------------------------------------------------
最近由于事情太多太多，所以发公众号的事情，我们就从简吧，有错别字或编辑不到位的地方，请各位海涵。
