px4源码(inertial_filter.cpp)

由于博客内容为空,暂无法提供包含关键信息的摘要。

摘要生成于 C知道 ,由 DeepSeek-R1 满血版支持, 前往体验 >

#include "px4_defines.h"
#include "inertial_filter.h"
#include <cmath>

//惯性滤波预测
// 预测函数
/*
 * 对于匀变速运动
 * 速度v = v0 + a*t
 * 位移s = v0*t + 0.5*a*t^2
 * 
*/

void inertial_filter_predict(float dt, float x[2], float acc)
{
	if (PX4_ISFINITE(dt)) {
		if (!PX4_ISFINITE(acc)) {
			acc = 0.0f;
		}

		x[0] += x[1] * dt + acc * dt * dt / 2.0f;
		x[1] += acc * dt;
	}
}

//惯性滤波矫正
// 校正函数
/*
 * e修正系数,dt周期时间,x[2]是两个float型成员,x[0]是位置,x[1]是速度
 * i表示修正位置还是速度:0修正位置,1修正速度
 * w权重系数
 * 
*/
void inertial_filter_correct(float e, float dt, float x[2], int i, float w)
{
	if (PX4_ISFINITE(e) && PX4_ISFINITE(w) && PX4_ISFINITE(dt)) {
		float ewdt = e * w * dt;
		x[i] += ewdt;

		if (i == 0) {
			x[1] += w * ewdt;
		}
	}
}
评论
添加红包

请填写红包祝福语或标题

红包个数最小为10个

红包金额最低5元

当前余额3.43前往充值 >
需支付:10.00
成就一亿技术人!
领取后你会自动成为博主和红包主的粉丝 规则
hope_wisdom
发出的红包
实付
使用余额支付
点击重新获取
扫码支付
钱包余额 0

抵扣说明:

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

余额充值