【APM】寻找APM中EKF的五大公式

EKF核心代码位置

AP_NavEKF2.cpp
在这里插入图片描述
在这里插入图片描述
进入该函数
## 标题
进入该函数,然后可以看到关键部分,也即卡尔曼五个公式的地方。
在这里插入图片描述
下面介绍每个公式的具体位置

28状态值

首先要知道选用的状态值有哪些,28状态值(但参与EKF方差估计的只有24个,不计算四元数的协方差,FIX4中的EKF是24状态值,没有0~2)
在这里插入图片描述

公式一:x(k+1|k)=F*x(k|k)+Q

四元数更新↓
在这里插入图片描述
速度位置更新↓
在这里插入图片描述
调用的相关变量如下↓
在这里插入图片描述
在这里插入图片描述
在这里插入图片描述
在这里插入图片描述

总结

IMU的数据用于预测过程,
IMU得到的角度增量用于更新四元数
IMU得到的速度增量用于更新速度和位置
更新姿态角涉及到状态值【9-14,上一时刻的26~28】
更新速度涉及到旋转矩阵及状态值【15】,也即涉及到状态值【9-15,上一时刻的26~28】
更新位置涉及到速度,也即涉及到状态值【3-5,9-15,上一时刻的26~28】
由此发现状态值的更新是非线性的,需要通过求偏导得到状态转移矩阵,和噪声驱动矩阵,从而进行P的第一次更新。

公式二:P(k+1|k) = FP(k|k)Ft + GQGt

进入下面该函数↓
在这里插入图片描述
得到过程噪声 ,注意没有四元数,共24维。其中进行了状态更新的速度位置和误差角,即状态量的【0~8】初始化为0
在这里插入图片描述
其他几个值分别初始化为↓
在这里插入图片描述
nextP即为更新后的P值,这里为最后的输出值,代码中完整公式为↓
在这里插入图片描述
nextP的来源就是根据上述公式,结果较长,这里不粘贴,知道是通过雅克比求偏导得来的就行。
NextP求完,复制给P
在这里插入图片描述

公式三:K=P(k+1|k) * Ht * 1/S(k)

下面只以磁力计为例
首先得到创新数据,也即H*stateStruct — MagMea,如下↓
在这里插入图片描述
其中MagPred,来源于状态值stateStruct.quat、stateStruct.earth_magfield、stateStruct.body_magfield↓
在这里插入图片描述
初始化测量噪声↓
在这里插入图片描述
根据公式得到K↓
在这里插入图片描述

公式四:x(k+1|k+1) = x(k+1|k) + K * (innovX)

在这里插入图片描述

公式五:P(k+1|k+1) =(I - K*H) P(k+1|k)

在这里插入图片描述
自此,卡尔曼的五个公式完成了,也得到了新的stateStruct值。
然而,最终采用的估计值并非是EKF的输出值,而是有用新的IMU的与EKF的输出值进行融合,作为最终的估计值,存放在outputState中。
APM的EKF花了大量的代码来限制噪声协方差的大小,以及根据计算的方差来判断是否进行融合等,更精华的地方或许也在此。

飞控学习告一段落,接下来学习slam了。

  • 2
    点赞
  • 7
    收藏
    觉得还不错? 一键收藏
  • 打赏
    打赏
  • 1
    评论
EKF(扩展卡尔曼滤波)是一种常用于状态估计的滤波算法,可以用于估计动态系统的状态变量。在电池管理系统,估计电池的剩余电荷状态(SOC)是一项重要的任务,而EKF可以用于预估SOC。下面是EKF预估SOC公式的推导过程: 1. 首先,定义电池的动态模型,可以表示为: SOC(k+1) = f(SOC(k), I(k)) 其,SOC(k)表示在时间k的电池SOC,I(k)表示在时间k的电流输入。 2. 利用泰勒展开近似公式对动态模型进行线性化处理,可以得到如下形式: SOC(k+1) ≈ f(SOC(k), I(k)) + A(k)(SOC(k) - SOC_hat(k)) + B(k)(I(k) - I_hat(k)) 其,SOC_hat(k)和I_hat(k)表示在时间k的SOC和电流输入的估计值,A(k)和B(k)是模型的雅可比矩阵。 3. 然后,定义传感器模型,可以表示为: V(k) = h(SOC(k)) 其,V(k)表示在时间k的电池电压,h(SOC(k))是SOC的非线性函数关系。 4. 利用泰勒展开近似公式对传感器模型进行线性化处理,可以得到如下形式: V(k) ≈ h(SOC(k)) + C(k)(SOC(k) - SOC_hat(k)) 其,C(k)是传感器模型的雅可比矩阵。 5. 对上述两个模型进行合并得到整体状态估计公式: SOC_hat(k+1) = f(SOC_hat(k), I(k)) + A(k)(SOC(k) - SOC_hat(k)) + B(k)(I(k) - I_hat(k)) V(k) = h(SOC_hat(k)) + C(k)(SOC(k) - SOC_hat(k)) 6. 接下来,利用卡尔曼滤波的推导公式,根据系统的状态方程和观测方程,通过递推的方式进行SOC的预估。 以上就是EKF预估SOC公式的推导过程。通过使用EKF算法,我们可以通过测量电池的电压和电流输入,来估计电池的剩余电荷状态。这对于电池管理系统的设计和控制非常重要,有助于提高电池的使用寿命和性能。

“相关推荐”对你有帮助么?

  • 非常没帮助
  • 没帮助
  • 一般
  • 有帮助
  • 非常有帮助
提交
评论 1
添加红包

请填写红包祝福语或标题

红包个数最小为10个

红包金额最低5元

当前余额3.43前往充值 >
需支付:10.00
成就一亿技术人!
领取后你会自动成为博主和红包主的粉丝 规则
hope_wisdom
发出的红包

打赏作者

Yoldfish

你的鼓励将是我创作的最大动力

¥1 ¥2 ¥4 ¥6 ¥10 ¥20
扫码支付:¥1
获取中
扫码支付

您的余额不足,请更换扫码支付或充值

打赏作者

实付
使用余额支付
点击重新获取
扫码支付
钱包余额 0

抵扣说明:

1.余额是钱包充值的虚拟货币,按照1:1的比例进行支付金额的抵扣。
2.余额无法直接购买下载,可以购买VIP、付费专栏及课程。

余额充值