其他分享
首页 > 其他分享> > TSLAM9--------中心差分卡尔曼滤波(cdkf)

TSLAM9--------中心差分卡尔曼滤波(cdkf)

作者:互联网

新一年的算法更新季又来了,这一次tbus的优化侧重点任然放在【状态估计】方面。
去年推出了tiny_ekf主要侧重点在飞的稳不稳方面,而今年由于业务量的增加,估计精度的需求也凸显出来。

我们直接进入主题:

一、Tslam9功能性更新-----适配镭神N301远距离激光雷达:

在这里插入图片描述

不得不说,镭神n301比思岚系列雷达精度高太多太多,我们的结构都常常跟我说,我们把标配改成镭神算了--!!但是L还是坚持,算了,N301还是太重了,5倍重量换精度,其实不值得,毕竟是飞机,还是列为可选件吧!--!!
*(请相信我不是他们的托,其实人家也看不起我们这种托)

下面给大家看一些他们官网上看不到的参数,都是tbus实测得出的:

总的来说,精度高距离远,由于重量大,用在大场景大飞机上是特别适合的。

二、Tslam9算法更新----新的状态估计器,中心差分卡尔曼滤波(cdkf)

客户给L出了一个题:请把脚架大小3030cm的无人机,通过二维码引导降落停在一个4545cm平台上。L一口答应!这种找角点的垃圾游戏,对L来说犹如吃饭喝水。
几天下来,代码哗哗写完,自信满满进入测试阶段,接下来的惨状,我就不多说了,脸打的啪啪响。
经过曲折离奇的调试,最终发现,其实问题出在状态估计上,在面临这种精确停靠的地方,需要做精确控制,由于状态给的不够准确,控制都很紊乱,那肯定停不好咯。

具体出现了以下几个问题:

  1. 无人机处于悬停状态一直调节,不落下
  2. 如果把容差范围设大,下落倒是可以很快了,但是误差会特别大,10次甚至有2-3次会超出范围
  3. 快接近目标水平坐标以后,微量的调节干脆直接就不响应

以上三个问题分别对应于:

  1. 速度估计不准确
  2. 位置估计不准确
  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!

实测1919维状态变量的情况下,计算耗时:cdkf=8ekf
并且随着维度的增加,比例还会增加,由于网上都是拿一维说事儿,所以当然差不多,甚至ukf比ekf还快。
所以,单片机的控,至少stm32f4系列是歇菜了,f7也许能战,h7肯定能战

4. 目前有比cdkf更好的状态估计方法吗?YES!

这是自然,这里有一张老外的图:
在这里插入图片描述

cdkf相对于ukf、3阶ckf表现效果更好,但是任然比不过5阶ckf(容积卡尔曼滤波)

5. 目前还有哪些优化空间呢?

总的来说评价状态估计有3个指标:

  1. 稳定性
  2. 精度
  3. 鲁棒性

精度提升空间已经不太大了,据我所知,下来最多换ckf-5th,但任然是基于sigma point的估计意思不大。
稳定性方面的话,还有很大空间,tbus一直都是在用直接卡尔曼滤波,其实对于旋转的估计,间接卡尔曼滤波是航空航天一直都在使用的方式,可以搜索:eskf或者mekf。
优势在这里:
在这里插入图片描述

鲁棒方面的话基本没做,说白了就是自适应。这方面的话不太了解,但是看论文提升是挺大的,GPS故障之类的都能保持稳定,下次升级也许会尝试。

标签:--------,状态,cdkf,ekf,卡尔曼滤波,估计,ukf
来源: https://blog.csdn.net/LINGTUO_TBUS/article/details/115481564