inavFilter 惯导融合算法

typedef struct estimatorStruct{float pos[3];float vel[3];float acc[3];}estimator_s;
/*** @brief: 根据位置加速度来计算位置和速度
Inertial filter, implementation taken from PX4 implementation by Anton Babushkin <rk3dov@gmail.com>* @param {int} axis* @param {float} dt* @param {float} acc* @return {*}*/
void inavFilterPredict(estimator_s *estimat, int axis, float dt, float acc)
{estimat->pos[axis] += estimat->vel[axis] * dt + acc * dt * dt / 2.0f; //s = s + v*t + 1/2*a*t^testimat->vel[axis] += acc * dt; //v=v+a*t
}
/*位置校正*/
void inavFilterCorrectPos(estimator_s *estimat, int axis, float dt, float error, float weight)
{float ewdt = error * weight * dt;estimat->pos[axis] += ewdt;estimat->vel[axis] += weight * ewdt;
}
/*速度校正*/
void inavFilterCorrectVel(estimator_s *estimat, int axis, float dt, float error, float weight)
{estimat->vel[axis] += error * weight * dt;
}

程序原理：

1. void inavFilterPredict(estimator_s *estimat, int axis, float dt, float acc)

通过加速度acc和时间dt的积分直接出算位移和速度，加速度计有误差时，积分就会不断的累计误差，速度就不可能为0，位移也会一直往一个方向增加。

2. void inavFilterCorrectPos(estimator_s *estimat, int axis, float dt, float error, float weight)

通过另外一个位移来修正上面的位移误差,那修正时就必须要知道误差是多少，所以要有error，如果另外一个位移计的准确度是100%，那weight就是1 ，结果加上这个error*1*dt就等于另外一个位移计了。准确度是不可能100%，那weight也不要大于1。weight就是另外一个位移计的权重/可信度。位移的修正值是e*w*dt,速度的修正值是e*w*dt*dt. 误差是不断变化的，所以修正修值也是不断变化的。误差是位移的误差。

3.void inavFilterCorrectVel(estimator_s *estimat, int axis, float dt, float error, float weight)

上面修正了位移和速度，但如果结果速度的修正还不够的话，同理也可以再给速度单独一个修正的权重。这时的误差是速度的误差，修正值是e*w*dt

inavFilter 惯导融合算法相关推荐

RF-LIO：面向高动态场景的紧耦合LiDAR惯导融合里程计（IROS 2021）
RF-LIO: 面向高动态场景的紧耦合LiDAR惯导融合里程计单位:西安交通大学针对问题: 实际场景中动态因素的引入造成基于静态假设的LIO严重位姿漂移提出方法: 提出基于自适应的多分辨率Ran ...
四旋翼惯导融合之观测传感器滞后问题汇总与巴特沃斯低通滤波器设计（气压计MS5611、GPS模块M8N、超声波、PX4FLOW等）
前文讲到APM的三阶互补方案,之前附的图是从学长博客里面抠的,感觉还不是很详细,于是自己就画了下,顺便重新理一下思路. 上图中下标为O的表示原始量(Origion),C表示矫正后的量(Correcti ...
AR设备单目视觉惯导SLAM算法综述与评价
点云PCL免费知识星球,点云论文速读. 标题:Survey and evaluation of monocular visual-inertial SLAM algorithms for augmen ...
WiFi与惯导融合定位其二：安卓获取惯性数据（线性加速度、旋转矢量）
运动传感器的可能架构因传感器类型而异: 重力.线性加速度.旋转矢量.有效运动.计步器和步测器传感器可能基于硬件,也可能基于软件(处理过的数据,可能为多个传感器数据的融合). 加速度计传感器和陀螺仪传感 ...
028捷联惯导更新算法备忘
1.姿态更新对于nnn及bbb系,假定有如下四元数转换关系: 那么: rn(k+1)=Cb(k+1)n(k+1)rb(k+1)=Cn(k)n(k+1)Cn(k)b(k)Cb(k)n(k)Cb ...
WiFi与惯导融合定位其一：通过安卓手机获取路由器数据（MAC、RSSI）
电磁类信号定位的方法和原理都大同小异,只是会因为频段的不同而产生了电磁信号不同的损耗特点.所以5G定位也好,WiFi定位也好,算法原理都是差不多,不同的是基站和频谱.对于WiFi定位的实验,先从这种最 ...
关于GPS、惯导、视觉里程计的几个定义
1.首先写几个定义: 惯性导航系统(Inertial Navigation System, INS) 全球定位卫星系统(Global Navigation Satellite System, ...
【点云论文速读】基于优化的视觉惯导里程计与GPS的紧耦合的融合方案
转载自:https://mp.weixin.qq.com/s/Y-h7eto1Zc_Mkzlh653vpg [点云论文速读]基于优化的视觉惯导里程计与GPS的紧耦合的融合方案原创 dianyunPC ...
组合导航算法（一）之捷联惯导更新及组合模式
捷联惯导基本算法惯性导航技术于20世纪50年代最初开始投入使用,可分为物理平台与模拟平台.物理平台就是平台式惯性导航系统(PINS).模拟平台又称捷联式惯性导航系统(SINS),它以计算机为平台,随 ...

inavFilter 惯导融合算法

inavFilter 惯导融合算法相关推荐

最新文章

热门文章