无人机姿态解算_扩展卡尔曼滤波(2)
一、扩展卡尔曼滤波
KF和EKF的公式对比(基本没差别)
二、扩展卡尔曼五个公式
利用扩展卡尔曼滤波估计四元数。
下图是论文中的截图。可以和前面的卡尔曼滤波估计高度文章的那五个公式对应一下。
观测矩阵的确定。
三、代码的实现
1. 四元数模长归一化
static void NormalizeQuat(arm_matrix_instance_f32 *_q)
{float norm = invSqrt(_q->pData[0] * _q->pData[0] + _q->pData[1] * _q->pData[1] + _q->pData[2]*_q->pData[2] + _q->pData[3]*_q->pData[3]);// 归一化四元数_q->pData[0] *= norm;_q->pData[1] *= norm;_q->pData[2] *= norm;_q->pData[3] *= norm;
}
2. 快速开平方求导
// Fast inverse square-root
// See: http://en.wikipedia.org/wiki/Fast_inverse_square_root
float invSqrt(float x) /*快速开平方求倒*/
{float halfx = 0.5f * x;float y = x;long i = *(long*)&y;i = 0x5f3759df - (i>>1);y = *(float*)&i;y = y * (1.5f - (halfx * y * y));return y;
}
3. EKF公式1,2
/* * 卡尔曼公式1,2--计算先验的_q_(四元数)和_P_(误差协方差矩阵)* 输入:机体角速度 w (rad/s), (3x1)* 时间步长 dt (s)* 上一次后验的四元数 _q (4x1)
* 上一次后验的协方差矩阵 _P (4x4):这里和加速度计有关* 过程噪声协方差矩阵Q (4x4)* 输出:先验状态,四元数 _q_ (4x1)* 误差协方差矩阵的先验估计 _P_ (4x4)*/
static void Km12(const Axis3f w, const float dt, const arm_matrix_instance_f32 _q, const arm_matrix_instance_f32 _P, const arm_matrix_instance_f32 _Q ,arm_matrix_instance_f32 _q_, arm_matrix_instance_f32 _P_)
{float32_t A[16];arm_matrix_instance_f32 _A;float32_t A_T[16] = {1,0,0,0, 0,1,0,0, 0,0,1,0, 0,0,0,1}; // 矩阵A的转置arm_matrix_instance_f32 _A_T;float32_t T[16] = {1,0,0,0, 0,1,0,0, 0,0,1,0, 0,0,0,1}; // 临时定义中间变量arm_matrix_instance_f32 _T;float32_t T2[16] = {1,0,0,0, 0,1,0,0, 0,0,1,0, 0,0,0,1}; // 临时定义中间变量arm_matrix_instance_f32 _T2;float temp;temp = dt / 2;A[0] = 1.0f; A[1] = -w.x*temp; A[2] = -w.y*temp; A[3] = -w.z*temp;A[4] = w.x*temp; A[5] = 1.0f; A[6] = w.z*temp; A[7] = -w.y*temp;A[8] = w.y*temp; A[9] = -w.z*temp; A[10] = 1.0f; A[11] = w.x*temp;A[12] = w.z*temp; A[13]= w.y*temp; A[14] = -w.x*temp; A[15] = 1.0f;arm_mat_init_f32(&_A, 4, 4, A); // 转移矩阵A (4x4)arm_mat_init_f32(&_A_T, 4, 4, A_T);arm_mat_init_f32(&_T, 4, 4, T);arm_mat_init_f32(&_T2, 4, 4, T2);arm_mat_trans_f32(&_A, &_A_T); // A' (4x4)arm_mat_mult_f32(&_A, &_P, &_T); // _T = _A*_P (4x4)arm_mat_mult_f32(&_T, &_A_T, &_T2); // _T2_ = _T*_A_T (4x4)arm_mat_add_f32(&_T2, &_Q, &_P_); // _P_ = _T2 + _Q = _A*_P*_A_T + _Q;误差协方差矩阵的先验估计公式arm_mat_mult_f32(&_A, &_q, &_q_); // q_ = A*q (4x1) xk的先验估计,没有控制项
}
4. EKF公式3
/** 卡尔曼公式3--计算卡尔曼增益* 输入:上一次估计四元数 _q (4x1)* 先验的误差协方差矩阵 _P_ (4x4)* 测量噪声协方差矩阵 _R (3x3)* 输出:卡尔曼增益 _K (4x3)*/
static void Km3(const arm_matrix_instance_f32 _q, const arm_matrix_instance_f32 _P_, const arm_matrix_instance_f32 _R, arm_matrix_instance_f32 _K)
{float32_t H[12]; // (3x4)float32_t H_T[12]; // (4x3)float32_t T[12]; // (3x4)float32_t T1[9]; // (3x3)float32_t M[9]; // (3x3)float32_t M_[9]; // M的逆, (3x3)arm_matrix_instance_f32 _H;arm_matrix_instance_f32 _H_T;arm_matrix_instance_f32 _T;arm_matrix_instance_f32 _T1;arm_matrix_instance_f32 _M;arm_matrix_instance_f32 _M_;// 用加速度计进行修正时的观测矩阵H[0] = -2*_q.pData[2]; H[1] = 2*_q.pData[3]; H[2] = -2*_q.pData[0]; H[3] = 2*_q.pData[1];H[4] = 2*_q.pData[1]; H[5] = 2*_q.pData[0]; H[6] = 2*_q.pData[3]; H[7] = 2*_q.pData[2];H[8] = 2*_q.pData[0]; H[9] = -2*_q.pData[1]; H[10] = -2*_q.pData[2]; H[11] = 2*_q.pData[3];// 矩阵初始化arm_mat_init_f32(&_H, 3, 4, H); // 观测矩阵H (3x4)arm_mat_init_f32(&_H_T, 4, 3, H_T);arm_mat_init_f32(&_T, 3, 4, T); // T (3x4)arm_mat_init_f32(&_M, 3, 3, M); // M (3x3)arm_mat_init_f32(&_M_, 3, 3, M_); // M_(3x3)arm_mat_init_f32(&_T1, 3, 3, T1);// 卡尔曼增益arm_mat_mult_f32(&_H, &_P_, &_T); // _T = _H*_P_ (3x4)arm_mat_trans_f32(&_H, &_H_T); // _H_T (4x3)arm_mat_mult_f32(&_T, &_H_T, &_T1); // _T1 = _H*_P_*_H_T (3x3)arm_mat_add_f32(&_T1, &_R, &_M); // _M = _H*_P_*_H_T + _R (3x3)arm_mat_inverse_f32(&_M, &_M_); // _M_ = inv(_H*_P_*_H_T + R) (3x3)arm_mat_init_f32(&_T, 4, 3, T); // 将T初始化为 4x3 矩阵arm_mat_mult_f32(&_P_, &_H_T, &_T); // _T = _P_ * _H_T (4x3)arm_mat_mult_f32(&_T, &_M_, &_K); // _K = _P_ * _H_T * inv(_H*_P_*_H_T + R); (4x3)
}
5. EKF公式4,5
/** 卡尔曼公式4,5* 输入:观测的状态 a (加速度单位为 N/s^2) (3x1)* 先验的四元数 _qi (4x1)* 上一次估计的 _q* 先验的噪声协方差矩阵 _P_* 卡尔曼增益 _K (4x3)* 输出:后验的四元数 _qo (4x1)* 后验的噪声协方差矩阵 _P (4x4)*/
static void Km45( const Axis3f a, const arm_matrix_instance_f32 _qi, const arm_matrix_instance_f32 _q, const arm_matrix_instance_f32 _P_, const arm_matrix_instance_f32 _K, arm_matrix_instance_f32 _qo, arm_matrix_instance_f32 _P)
{float norm ; // 模长float32_t z[3]; // 观测的向量float32_t h[3]; // 地球系向量在机体系下的分量 (3x1)float32_t v3[3]; // 临时向量 (3x1)float32_t v4[4]; // 临时向量(4x1)float32_t I[16]= {1,0,0,0, 0,1,0,0, 0,0,1,0, 0,0,0,1}; // 单位阵(4x4)float32_t H[12];float32_t T[16];float32_t M[16];arm_matrix_instance_f32 _z;arm_matrix_instance_f32 _h;arm_matrix_instance_f32 _v3;arm_matrix_instance_f32 _v4;arm_matrix_instance_f32 _I ;arm_matrix_instance_f32 _H;arm_matrix_instance_f32 _T;arm_matrix_instance_f32 _M;// 获取测量值z[0] = a.x;z[1] = a.y;z[2] = a.z;// 矩阵初始化arm_mat_init_f32(&_z, 3, 1, z);arm_mat_init_f32(&_h, 3, 1, h);arm_mat_init_f32(&_v3, 3, 1, v3);arm_mat_init_f32(&_v4, 4, 1, v4);arm_mat_init_f32(&_I, 4, 4, I);arm_mat_init_f32(&_H, 3, 4, H);arm_mat_init_f32(&_T, 4, 4, T);arm_mat_init_f32(&_M, 4, 4, M);norm = invSqrt(a.x*a.x + a.y*a.y + a.z * a.z);z[0] *= norm;z[1] *= norm;z[2] *= norm;h[0] = 2*_q.pData[1]*_q.pData[3] - 2*_q.pData[0]*_q.pData[2];h[1] = 2*_q.pData[0]*_q.pData[1] + 2*_q.pData[2]*_q.pData[3];h[2] = _q.pData[0]*_q.pData[0] - _q.pData[1]*_q.pData[1] - _q.pData[2]*_q.pData[2] + _q.pData[3]*_q.pData[3]; // 观测矩阵H[0] = -2*_q.pData[2]; H[1] = 2*_q.pData[3]; H[2] = -2*_q.pData[0]; H[3] = 2*_q.pData[1];H[4] = 2*_q.pData[1]; H[5] = 2*_q.pData[0]; H[6] = 2*_q.pData[3]; H[7] = 2*_q.pData[2];H[8] = 2*_q.pData[0]; H[9] = -2*_q.pData[1]; H[10] = -2*_q.pData[2]; H[11] = 2*_q.pData[3];// 修正qarm_mat_sub_f32(&_z, &_h, &_v3); // _v3 = _z - _h (3x1)arm_mat_mult_f32(&_K, &_v3, &_v4); // _v4 = _K*(_z-_h) (4x1)_v4.pData[3] = 0; // 加速度计修正,不改变 q3。arm_mat_add_f32(&_qi, &_v4, &_qo); // _qo = _qi + _v4 (4x4)// 修正Parm_mat_mult_f32(&_K, &_H, &_T); // _T = _K*_H (4x4)arm_mat_sub_f32(&_I, &_T, &_M); // _M = _I - _K*_H (4x4)arm_mat_mult_f32(&_M, &_P_, &_P); // _P = (_I - _K*_H) * _P_
}
6. 应用扩展卡尔曼滤波进行6自由度的姿态解算
/** 卡尔曼滤波姿态估计* 输入:六轴传感器数据 ,加速度 m/s^2, 加速度 rad/s,* 此函数将更新全局变量 姿态四元数 quat_ 与 欧拉角 eular_*/
void ekfdof6(Axis3f acc, Axis3f gyro, state_t *state , float dt)
{float accBuf[3] = {0.f};Axis3f tempacc = acc;// 必须进行转换,否则微小的变动就会导致姿态变化很大。gyro.x = gyro.x * DEG2RAD; /* 度转弧度 */gyro.y = gyro.y * DEG2RAD;gyro.z = gyro.z * DEG2RAD;/* 卡尔曼滤波相关矩阵 */static float32_t Q[16] = {1e-5f,0,0,0, 0,1e-5f,0,0, 0,0,1e-5f,0, 0,0,0,1e-5f}; // 过程噪声协方差矩阵 4X4static float32_t R[9] = {1,0,0, 0,1,0, 0,0,1}; // 加速度计测量噪声协方差矩阵static float32_t q[4] = {1, 0, 0, 0}; // 使用加速度计后验得到的协方差矩阵static float32_t q_[4] = {1, 0, 0, 0}; // 先验的qstatic float32_t P_[16] = {1,0,0,0, 0,1,0,0, 0,0,1,0, 0,0,0,1}; // 先验的误差协方差矩阵的值static float32_t P1[16] = {1,0,0,0, 0,1,0,0, 0,0,1,0, 0,0,0,1}; // 通过加速度计后验得到的误差协方差矩阵static float32_t K[12]; // 卡尔曼增益/* 指向卡尔曼滤波相关矩阵的指针 */static arm_matrix_instance_f32 _Q;static arm_matrix_instance_f32 _R;static arm_matrix_instance_f32 _q; // xk的后验估计static arm_matrix_instance_f32 _q_; // xk的先验估计static arm_matrix_instance_f32 _P_; // 误差协方差矩阵的先验估计static arm_matrix_instance_f32 _P1;static arm_matrix_instance_f32 _K;static uint16_t km_atti_times = 0;/* 初始化卡尔曼滤波相关矩阵 */if (km_atti_times == 0){arm_mat_init_f32(&_Q, 4, 4, Q);arm_mat_init_f32(&_R, 3, 3, R);arm_mat_init_f32(&_q, 4, 1, q);arm_mat_init_f32(&_q_, 4, 1, q_);arm_mat_init_f32(&_P_, 4, 4, P_);arm_mat_init_f32(&_P1, 4, 4, P1);arm_mat_init_f32(&_K, 4, 3, K);km_atti_times = 1;}else if (km_atti_times < 1000) // 先信任加速度计,收敛快{Q[0] = 8e-3f;Q[5] = 8e-3f;Q[10] =8e-3f;Q[15] = 8e-3f;km_atti_times ++;}else // 后信任陀螺仪,准确{Q[0] = 1e-7f;Q[5] = 1e-7f;Q[10] = 1e-7f;Q[15] = 1e-7f; }/* 卡尔曼滤波过程 */Km12(gyro, dt, _q, _P1, _Q, _q_, _P_); // 卡尔曼公式1,2 根据陀螺仪预测Km3(_q, _P_, _R, _K); // 卡尔曼公式3,计算卡尔曼增益Km45(acc, _q_, _q, _P_, _K, _q, _P1); // 卡尔曼公式4,5 根据加速度修正NormalizeQuat(&_q); // 四元数归一化/* 赋值给全局变量 */q0 = _q.pData[0]; q1 = _q.pData[1];q2 = _q.pData[2];q3 = _q.pData[3];imuComputeRotationMatrix(); /*计算旋转矩阵*//*计算roll pitch yaw 欧拉角*/state->attitude.pitch = -asinf(rMat[2][0]) * RAD2DEG; state->attitude.roll = atan2f(rMat[2][1], rMat[2][2]) * RAD2DEG;state->attitude.yaw = atan2f(rMat[1][0], rMat[0][0]) * RAD2DEG;if (!isGravityCalibrated) /*未校准*/{ accBuf[2] = tempacc.x* rMat[2][0] + tempacc.y * rMat[2][1] + tempacc.z * rMat[2][2];/*accz*/calBaseAcc(accBuf); /*计算静态加速度*/ }
}
7. 计算旋转矩阵
/*计算旋转矩阵*/
void imuComputeRotationMatrix(void)
{float q1q1 = q1 * q1;float q2q2 = q2 * q2;float q3q3 = q3 * q3;float q0q1 = q0 * q1;float q0q2 = q0 * q2;float q0q3 = q0 * q3;float q1q2 = q1 * q2;float q1q3 = q1 * q3;float q2q3 = q2 * q3;rMat[0][0] = 1.0f - 2.0f * q2q2 - 2.0f * q3q3;rMat[0][1] = 2.0f * (q1q2 + -q0q3);rMat[0][2] = 2.0f * (q1q3 - -q0q2);rMat[1][0] = 2.0f * (q1q2 - -q0q3);rMat[1][1] = 1.0f - 2.0f * q1q1 - 2.0f * q3q3;rMat[1][2] = 2.0f * (q2q3 + -q0q1);rMat[2][0] = 2.0f * (q1q3 + -q0q2);rMat[2][1] = 2.0f * (q2q3 - -q0q1);rMat[2][2] = 1.0f - 2.0f * q1q1 - 2.0f * q2q2;
}
四、移植到MiniFly微型四轴中
6 , MiniFly V1.5\1,MiniFly Master\2,STM32F411\Firmware_F411 V1.4(改灯\FLIGHT\src\stabilizer.c
//四元数和欧拉角计算(250Hz)if (RATE_DO_EXECUTE(ATTITUDE_ESTIMAT_RATE, tick)){// imuUpdate(sensorData.acc, sensorData.gyro, &state, ATTITUDE_ESTIMAT_DT);ekfdof6(sensorData.acc, sensorData.gyro, &state, ATTITUDE_ESTIMAT_DT); }
五、参考论文
A Double-Stage Kalman Filter for Orientation Tracking With an Integrated Processor in 9-D IMU
无人机姿态解算_扩展卡尔曼滤波(2)相关推荐
- 基于STM32F407四旋翼无人机 --- 姿态解算讲解(四元数)(叉积法融合陀螺仪数据和加速度数据)(五)
基于STM32F407四旋翼无人机 --- 姿态解算讲解(五) 姿态解算 姿态解算定义 欧拉角 四元数 四元数性质 方向余弦矩阵 四元数方向余弦矩阵 叉积法融合陀螺仪数据和加速度数据 叉积运算 一阶龙 ...
- 无人机姿态解算:四元数及其与欧拉角的转换
无人机姿态解算:四元数及其与欧拉角的转换 引言:获得无人机飞行时的飞行姿态对于无人机稳定控制来说至关重要.无人机主要通过传感器数据融合来进行状态估计,常用于无人机的传感器包括:MPU(包含了三轴加速度 ...
- UAV012_V2(二):无人机姿态解算(深入篇)
写这篇博客,已经是第三次了,花了一个周,一遍遍修改,只为了理解好姿态解算并表述出来. 之前写过一篇姿态解算的博客,UAV021(四):飞控传感器数据融合与姿态估计,在小角度假设条件(俯仰角.滚转角都很 ...
- python捷联惯导的姿态解算_自动驾驶中高精地图的大规模生产:视觉惯导技术在高德的应用...
导读:导航.驾驶辅助.自动驾驶等技术的不断发展对地图的精细程度提出了更高的要求.常规的道路级地图对于智能交通系统存在很多不足,针对自动驾驶应用的需求,我们提出了利用视觉惯导技术制作高精地图的方法. 本 ...
- 基于四元数互补滤波的无人机姿态解算
导航坐标系为东北天(ENU),其与机体坐标系(b)的方向余弦矩阵为CbcC_{b}^{c}Cbc
- 卡尔曼滤波五个公式_基于ROS的卡尔曼滤波姿态解算
前段时间由于项目关系需要实现基于卡尔曼滤波的姿态解算,也就是说融合加速度计.陀螺仪及磁罗盘进行AHRS姿态解算,得到机器人的姿态角. 本文的学习需要有一定的卡尔曼滤波器基础,可以参考白巧克力亦唯心的卡 ...
- Pixhawk之姿态解算篇(5)_ECF/EKF/GD介绍
一.开篇 很久没更新blog了, 最近研究的东西比较杂乱,也整理了很多东西,没有来的及更新,最近发现很多小伙伴都开始写blog了,在不更新就要"被落后了".兄弟们,等等我啊~~~ ...
- 驱动LSM6DS3TR-C实现高效运动检测与数据采集(5)----姿态解算
概述 lsm6ds3trc包含三轴陀螺仪与三轴加速度计. 姿态有多种数学表示方式,常见的是四元数,欧拉角,矩阵和轴角.他们各自有其自身的优点,在不同的领域使用不同的表示方式.在四轴飞行器中使用到了四元 ...
- Pixhawk之姿态解算篇(4)_补充篇
一.开篇 大家期待已久的第四篇来了,但是本篇可能比较水啊~~~见谅~~~ 首先,上一周没有什么收获,虽然看了不少的论文,但是却没有什么质的飞越~~~~ 看的论文都是关于姿态解算的,用的算法大部分也都是 ...
最新文章
- linux date
- SQL SERVER 查看并结束某个进程
- LyaoutParameters作用
- mysql 加密方式 caching_sha2_password 和 mysql_native_password 说明
- c++函数重载机制实现原理
- git使用的基本流程_git命令的基本使用
- LeetCode 105. Construct Binary Tree from Preorder and Inorder Traversal 由前序和中序遍历建立二叉树 C++...
- 前端学习(2824):数据绑定前的代码编辑器技巧
- nginx将ip+端口号映射为域名
- java面向对象基础代码_JAVA基础知识点之Java面向对象
- 文件拷贝(字符、字节)
- 支付宝超硬硬件发布: 将颠覆现有支付方式!
- FAQ系列 | mysqldump选项之skip-opt
- 受宠的背后:安全市场面临重新洗牌
- mysql数据库cms数据库文件_PbootCMS 默认数据库转Mysql数据库教程 - 老蒋SEO博客
- Matlab电气课程设计,电气工程课程设计——基于Matlab异步电动机调速系统设计
- maven源码阅读之一(Guice介绍)
- 实验2 运算器的编程实现
- win10 mysql 卸载不干净,安装提示,已经存在
- vi中多个文件相互之间的复制、粘贴功能简介!
热门文章
- 选择尚学堂与传智播客的java培训?
- 局域网访问电脑中VMware虚拟机
- github获取token
- Servlet设置欢迎页面!
- pytorch 定义torch类型数据_PyTorch 使用TorchText进行文本分类
- SpringBoot导入导出你会用吗?(EasyPoi)
- 3D segmentation of nasopharyngeal carcinoma from CT images using cascade deep learning
- Mapbox Style 规范(中文)
- 第一次冲刺--查看活动详情用户场景分析
- findfirst, findnext