【机器人项目】invSqrt快速解算平方根,效率高

今天在别人的源码里看到一个很不错的函数,初次还没进入该函数看,以为就是一个普通的开方根函数,但进入后发现该函数竟然是著名的Carmack平方根求解算法,拥有极高的运算效率,堪称经典。号称比标准的sqrt()函数快4倍!赶紧记录下来以后用!​​​​​​​

//函数名:invSqrt(void)
//描述:求平方根的倒数
//该函数是经典的Carmack求平方根算法,效率极高,使用魔数0x5f375a86
static float invSqrt(float number)
{
    volatile long i;
    volatile float x, y;
    volatile const float f = 1.5F;

    x = number * 0.5F;
    y = number;
    i = * (( long * ) &y);
    i = 0x5f375a86 - ( i >> 1 );
    y = * (( float * ) &i);
    y = y * ( f - ( x * y * y ) );
    return y;//----得到平方根的倒数           //return 1/y -----得到平方根
}
  • 0
    点赞
  • 1
    收藏
    觉得还不错? 一键收藏
  • 0
    评论
Mahony算法是一种用来融合加速度计和陀螺仪数据的算法,用于估算机器人或无人机的姿态。它通常由以下几个步骤组成: 1. 利用加速度计读数计算出重力向量。 2. 利用陀螺仪读数计算出角速度向量。 3. 使用某种滤波方法,如卡尔曼滤波或梯度下降法,融合这两个向量来估计姿态。 4. 在陀螺仪数据上使用误差积分来修正姿态估计值。 这里是一个用C++编写的Mahony算法的例子代码: ```c++ #define Kp 2.0f // proportional gain governs rate of convergence to accelerometer/magnetometer #define Ki 0.005f // integral gain governs rate of convergence of gyroscope biases float q0 = 1.0f, q1 = 0.0f, q2 = 0.0f, q3 = 0.0f; // quaternion of sensor frame relative to auxiliary frame void MahonyAHRSupdateIMU(float gx, float gy, float gz, float ax, float ay, float az) { float recipNorm; float halfvx, halfvy, halfvz; float halfex, halfey, halfez; float qa, qb, qc; // Compute feedback only if accelerometer measurement valid (avoids NaN in accelerometer normalisation) if(!((ax == 0.0f) && (ay == 0.0f) && (az == 0.0f))) { // Normalise accelerometer measurement recipNorm = invSqrt(ax * ax + ay * ay + az * az); ax *= recipNorm; ay *= recipNorm; az *= recipNorm; // Estimated direction of gravity halfvx = q1 * q3 - q0 * q2; halfvy = q0 * q1 + q2 * q3; halfvz = q0 * q0 - 0.5f + q3 * q3; // Error is sum of cross product between estimated direction and measured direction of field vectors halfex = (ay * halfvz - az * halfvy); halfey = (az * halfvx - ax * halfvz); half
评论
添加红包

请填写红包祝福语或标题

红包个数最小为10个

红包金额最低5元

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

抵扣说明:

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

余额充值