开源飞控OpenPilot的扩展卡尔曼滤波EKF学习笔记(三)
2014-10-22 19:10
106 查看
接着说,看else部分。
函数中重点是以上两句话,其余都是赋值与被赋值,不去看,首先看第一句,字面上看是线性化某物,再进入函数看一下
其中F矩阵称为系统矩阵,13×13维,就是我们的A阵,状态转移矩阵的线性化矩阵,对应我们的13维的状态变量,简单的说状态变量经过状态矩阵后就是下一步的状态变量。
G称为输入矩阵,13×9维,这里的输入是噪声,噪声和状态变量不一样,是个9×1维的矩阵,分别是三维陀螺仪噪声,三维加计噪声以及三维的偏差随机游走噪声(不知道什么偏差)。
我们知道,依据老外的现代控制理论,卡尔曼滤波的状态转移方程是:dotX=F*X+G*W。bolg不好写公式。
我们这里的F和G,就是上面公式里F和G的线性化。
好,LinearizeFG函数就这样吧,深入的我也不懂,反正这两个矩阵就这样了,拿过来用就可以了。
接着看RungeKutta函数,这个函数不用看也知道干啥的,龙格库塔解微分方程组,我进去看了一下,4阶定步长。
然后再跳出来,INSStatePrediction函数也走完了。
而else部分的剩余又是赋值,将解算完毕的状态变量赋值到外部可引用的数据结构中。然后else就结束了。
回头看一下,else部分只做了一件事,所谓的状态预测,就是解了一步微分方程组,也就是将陀螺仪和加计的数据积分一步,积分数据在长时间来说是不可信的,这个时间就MEMS陀螺仪来说不会很长,几秒或再长一点,就需要采用GPS,磁航向数据修正误差,让积分能够继续下去,修正部分还没看到,以后再说。
这篇博文拖了好长时间,主要是因为雅可比线性化的问题,也就是F矩阵里具体项的含义,这个矩阵表征物理模型,对滤波精度有很大影响,虽说大家都用一样的矩阵,但还是弄明白的好,我的打算是整个滤波流程走完了之后,再单独理解一下F矩阵。
好,先写到这。
float gyros[3] = { DEG2RAD(this->work.gyro[0]), DEG2RAD(this->work.gyro[1]), DEG2RAD(this->work.gyro[2]) }; INSStatePrediction(gyros, this->work.accel, dT);先看前两句,第一句不用说,给gyros[3]数组赋值,重点看第二句,从字面来看,是进行INS的状态预测,事实也是如此,进入到函数内部去看
LinearizeFG(ekf.X, U, ekf.F, ekf.G); RungeKutta(ekf.X, U, dT);
函数中重点是以上两句话,其余都是赋值与被赋值,不去看,首先看第一句,字面上看是线性化某物,再进入函数看一下
float ax, ay, az, wx, wy, wz, q0, q1, q2, q3; // ax=U[3]-X[13]; ay=U[4]-X[14]; az=U[5]-X[15]; // subtract the biases on accels ax = U[3]; ay = U[4]; az = U[5]; // NO BIAS STATES ON ACCELS wx = U[0] - X[10]; wy = U[1] - X[11]; wz = U[2] - X[12]; // subtract the biases on gyros q0 = X[6]; q1 = X[7]; q2 = X[8]; q3 = X[9]; // Pdot = V F[0][3] = F[1][4] = F[2][5] = 1.0f; // dVdot/dq F[3][6] = 2.0f * (q0 * ax - q3 * ay + q2 * az); F[3][7] = 2.0f * (q1 * ax + q2 * ay + q3 * az); F[3][8] = 2.0f * (-q2 * ax + q1 * ay + q0 * az); F[3][9] = 2.0f * (-q3 * ax - q0 * ay + q1 * az); F[4][6] = 2.0f * (q3 * ax + q0 * ay - q1 * az); F[4][7] = 2.0f * (q2 * ax - q1 * ay - q0 * az); F[4][8] = 2.0f * (q1 * ax + q2 * ay + q3 * az); F[4][9] = 2.0f * (q0 * ax - q3 * ay + q2 * az); F[5][6] = 2.0f * (-q2 * ax + q1 * ay + q0 * az); F[5][7] = 2.0f * (q3 * ax + q0 * ay - q1 * az); F[5][8] = 2.0f * (-q0 * ax + q3 * ay - q2 * az); F[5][9] = 2.0f * (q1 * ax + q2 * ay + q3 * az); // dVdot/dabias & dVdot/dna - NO BIAS STATES ON ACCELS - S0 REPEAT FOR G BELOW // F[3][13]=G[3][3]=-q0*q0-q1*q1+q2*q2+q3*q3; F[3][14]=G[3][4]=2*(-q1*q2+q0*q3); F[3][15]=G[3][5]=-2*(q1*q3+q0*q2); // F[4][13]=G[4][3]=-2*(q1*q2+q0*q3); F[4][14]=G[4][4]=-q0*q0+q1*q1-q2*q2+q3*q3; F[4][15]=G[4][5]=2*(-q2*q3+q0*q1); // F[5][13]=G[5][3]=2*(-q1*q3+q0*q2); F[5][14]=G[5][4]=-2*(q2*q3+q0*q1); F[5][15]=G[5][5]=-q0*q0+q1*q1+q2*q2-q3*q3; // dqdot/dq F[6][6] = 0; F[6][7] = -wx / 2.0f; F[6][8] = -wy / 2.0f; F[6][9] = -wz / 2.0f; F[7][6] = wx / 2.0f; F[7][7] = 0; F[7][8] = wz / 2.0f; F[7][9] = -wy / 2.0f; F[8][6] = wy / 2.0f; F[8][7] = -wz / 2.0f; F[8][8] = 0; F[8][9] = wx / 2.0f; F[9][6] = wz / 2.0f; F[9][7] = wy / 2.0f; F[9][8] = -wx / 2.0f; F[9][9] = 0; // dqdot/dwbias F[6][10] = q1 / 2.0f; F[6][11] = q2 / 2.0f; F[6][12] = q3 / 2.0f; F[7][10] = -q0 / 2.0f; F[7][11] = q3 / 2.0f; F[7][12] = -q2 / 2.0f; F[8][10] = -q3 / 2.0f; F[8][11] = -q0 / 2.0f; F[8][12] = q1 / 2.0f; F[9][10] = q2 / 2.0f; F[9][11] = -q1 / 2.0f; F[9][12] = -q0 / 2.0f; // dVdot/dna - NO BIAS STATES ON ACCELS - S0 REPEAT FOR G HERE G[3][3] = -q0 * q0 - q1 * q1 + q2 * q2 + q3 * q3; G[3][4] = 2.0f * (-q1 * q2 + q0 * q3); G[3][5] = -2.0f * (q1 * q3 + q0 * q2); G[4][3] = -2.0f * (q1 * q2 + q0 * q3); G[4][4] = -q0 * q0 + q1 * q1 - q2 * q2 + q3 * q3; G[4][5] = 2.0f * (-q2 * q3 + q0 * q1); G[5][3] = 2.0f * (-q1 * q3 + q0 * q2); G[5][4] = -2.0f * (q2 * q3 + q0 * q1); G[5][5] = -q0 * q0 + q1 * q1 + q2 * q2 - q3 * q3; // dqdot/dnw G[6][0] = q1 / 2.0f; G[6][1] = q2 / 2.0f; G[6][2] = q3 / 2.0f; G[7][0] = -q0 / 2.0f; G[7][1] = q3 / 2.0f; G[7][2] = -q2 / 2.0f; G[8][0] = -q3 / 2.0f; G[8][1] = -q0 / 2.0f; G[8][2] = q1 / 2.0f; G[9][0] = q2 / 2.0f; G[9][1] = -q1 / 2.0f; G[9][2] = -q0 / 2.0f; // dwbias = random walk noise G[10][6] = G[11][7] = G[12][8] = 1.0f;好长一段代码,全贴出来是因为没法简单的说,代码就干一件事,给F和G矩阵赋值,这两个矩阵是雅可比矩阵,雅可比是非线性系统线性化的一种方法,基本我看过的卡尔曼程序都是这个方法,具体怎么算我也不知道,单位同事说好像挺复杂,反正这个矩阵就是这样了。
其中F矩阵称为系统矩阵,13×13维,就是我们的A阵,状态转移矩阵的线性化矩阵,对应我们的13维的状态变量,简单的说状态变量经过状态矩阵后就是下一步的状态变量。
G称为输入矩阵,13×9维,这里的输入是噪声,噪声和状态变量不一样,是个9×1维的矩阵,分别是三维陀螺仪噪声,三维加计噪声以及三维的偏差随机游走噪声(不知道什么偏差)。
我们知道,依据老外的现代控制理论,卡尔曼滤波的状态转移方程是:dotX=F*X+G*W。bolg不好写公式。
我们这里的F和G,就是上面公式里F和G的线性化。
好,LinearizeFG函数就这样吧,深入的我也不懂,反正这两个矩阵就这样了,拿过来用就可以了。
接着看RungeKutta函数,这个函数不用看也知道干啥的,龙格库塔解微分方程组,我进去看了一下,4阶定步长。
然后再跳出来,INSStatePrediction函数也走完了。
而else部分的剩余又是赋值,将解算完毕的状态变量赋值到外部可引用的数据结构中。然后else就结束了。
回头看一下,else部分只做了一件事,所谓的状态预测,就是解了一步微分方程组,也就是将陀螺仪和加计的数据积分一步,积分数据在长时间来说是不可信的,这个时间就MEMS陀螺仪来说不会很长,几秒或再长一点,就需要采用GPS,磁航向数据修正误差,让积分能够继续下去,修正部分还没看到,以后再说。
这篇博文拖了好长时间,主要是因为雅可比线性化的问题,也就是F矩阵里具体项的含义,这个矩阵表征物理模型,对滤波精度有很大影响,虽说大家都用一样的矩阵,但还是弄明白的好,我的打算是整个滤波流程走完了之后,再单独理解一下F矩阵。
好,先写到这。
相关文章推荐
- 开源飞控OpenPilot的扩展卡尔曼滤波EKF学习笔记(四)
- 开源飞控OpenPilot的扩展卡尔曼滤波EKF学习笔记(二)
- 开源DataGridView扩展(4) 自定义带序号的行首
- 最流行的开源飞控项目ArduPilot Mega(APM)介绍及发展历史
- 开源飞控APM与PIXHAWK
- 开源MFC扩展界面库:Ultimate Toolbox的编译
- 开源软件架构:可扩展的Web架构与分布式系统
- Android项目:使用pulltorefresh开源项目扩展为下拉刷新上拉加载更多的处理方法,监听listview滚动方向
- 开源工程系列之EP3C25Q240扩展板1
- 开源DataGridView扩展(3) 单元格格式化器的设计 DataGridCellFormatter
- 开源DataGridView扩展(1) 扩展支持全选的CheckBox列。
- MFC扩展库UltimateToolbox 开源
- 谷歌扩展其开源承诺
- 用开源飞控套件做一架Mini四轴飞行器
- .net下开源轻量级ORM框架Dapper扩展系列4(重构与优化)
- 开源控件JazzyViewPager分析、扩展(支持循环)
- 大型web2.0互动网站设计方案:分析mixi.jp and Yeejee.com:用开源搭建的可扩展大型SNS网站
- 基于.NET 2.0的GIS开源项目SharpMap分析手记(八):怎样对SharpMap进行扩展开发——从许可角度来谈
- Twproject Gantt开源甘特图功能扩展
- 开源DataGridView扩展(5) 简单实现统计行,有更好的方法吗?