捷联惯导对位置进行求解，漂移太大？附件提供卡尔曼滤波
在不采用GPS卡尔曼组合滤波的情况下，有什么方法可以对惯性导航心痛进行经纬度和高度解算？只需要1分钟内有1m的精度都行。我现在采用的方法：
1.采用论坛上常用的四元素法实时更新欧拉角（加速度计磁阻实时补偿陀螺仪）；
2.通过欧拉角或者四元素求解出机体坐标系到当地地理坐标系的转换矩阵Cbn；
3.将加速度计测试出来的比力值，左乘转移矩阵，分解到当地地理坐标系下的比力值，并将D轴的加速度减去重力加速度值；
4.分别根据正北方的加速度、正东方的加速、重力方向的加速度值，2次积分求解得到经纬度和高度位置；
该方法的漂移太大了，10s能漂移30m，大神有做过位置导航的不？有没有更好的办法呢。
再次我给大家共享一个卡尔曼滤波程序，在STM32上成功运行，并取得一定的滤波效果。
虽然我还没有弄到这个阶段，但是我有个假想：
是不是 四轴的四个电机之间的距离 或者方位也对这个漂移有影响呢？
期待 大神解答。 不可能的，捷连只能求解姿态，位置还是需要引入新的观察者：GPS，因为加速度积分误差太大 你可能感觉自己的要求不过分，但是我可以明确告诉你，1分钟漂移1m,用惯性来做简直是白日做梦。就算你用上GPS，如果不是RTK和差分，都没戏。 qwe2231695 发表于 2013-4-28 01:04 static/image/common/back.gif
不可能的，捷连只能求解姿态，位置还是需要引入新的观察者：GPS，因为加速度积分误差太大 ...
我只追求短时间内的捷联位置求解，不超过5分钟的。气压计也可以对离海平面的高度进行补偿的，但是只能补偿单根轴，即地理坐标系的D轴！地理坐标系的N、E轴期待除了GPS的其它观测者？ asha 发表于 2013-4-28 08:42 static/image/common/back.gif
你可能感觉自己的要求不过分，但是我可以明确告诉你，1分钟漂移1m,用惯性来做简直是白日做梦。就算你用上GP ...
多谢解答，1分钟漂移1m的精度确实很高，1分钟10m也可以接受的~我在想能不能通过更好的传感器来提高精度！现在正在调试直接输出模电电压的，加速度传感器：MMA7361、陀螺仪：3个ENC-03MB。这样可以利用cpu进行高速采集并通过软件滤波（如卡尔曼滤波）！从而不受数字量输出传感器I2C通信速度的影响 楼主，你的这个卡尔曼怎么用的，我在程序里调用返回值怎么恒定为1呢 zb000000 发表于 2013-5-3 16:54 static/image/common/back.gif
多谢解答，1分钟漂移1m的精度确实很高，1分钟10m也可以接受的~我在想能不能通过更好的传感器来提高精度！ ...
你选的这几个传感器根本不行，就算用ADIS16355也达不到你的要求。我给你个数据，现在国内首家100W左右的惯导，纯惯性漂移是1小时一海里。 你看下你选的器件行么？还是选择价格GPS吧 nnnkey 发表于 2013-5-3 18:00 static/image/common/back.gif
楼主，你的这个卡尔曼怎么用的，我在程序里调用返回值怎么恒定为1呢
哈哈，我做个广告。还是买我AHRS开发板吧，带卡尔曼滤波程序，完全提供源代码。 保证是完整版本，而且是真正的四状态量，3观测量的。 可以移植到其它系统 nnnkey 发表于 2013-5-3 18:00 static/image/common/back.gif
楼主，你的这个卡尔曼怎么用的，我在程序里调用返回值怎么恒定为1呢
现在main函数中调用kalman初始化函数“KalMan_PramInit()”，然后用一个Timer固定一个5ms的周期（这个周期可以根据自己的cpu反应速度和传感器数据传输速度来调节）调用函数“float KalMan_Update(float *Z)”，该函数的输入参数“Z”是当前的测量值，返回值即是经过卡尔曼滤波后的值。里面影响最大就是Q和R两个调节参数，程序里面有注释 asha 发表于 2013-5-3 19:19 static/image/common/back.gif
哈哈，我做个广告。还是买我AHRS开发板吧，带卡尔曼滤波程序，完全提供源代码。 保证是完整版本，而且是 ...
我的姿态解算出来的欧拉角精度也是1°。姿态解算程序采用德国人的“AHRS.c”程序即可，很好用。将陀螺仪作为测量值，将加速度计和磁阻作为补偿值对测量值进行一定的补偿，解算出来的姿态漂移很少 nnnkey 发表于 2013-5-3 18:00 static/image/common/back.gif
楼主，你的这个卡尔曼怎么用的，我在程序里调用返回值怎么恒定为1呢
给你提供可德国人的AHRS姿态解算源码，未修改！/*
* AHRS
* Copyright 2010 S.O.H. Madgwick
*
* This program is free software: you can redistribute it and/or
* modify it under the terms of the GNU Lesser Public License as
* published by the Free Software Foundation, either version 3 of the
* License, or (at your option) any later version.
*
* This program is distributed in the hope that it will be useful, but
* WITHOUT ANY WARRANTY; without even the implied warranty of
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.See the GNU
* Lesser Public License for more details.
*
* You should have received a copy of the GNU Lesser Public License
* along with this program.If not, see
* <http://www.gnu.org/licenses/>.
*/
// AHRS.c
// S.O.H. Madgwick
// 25th August 2010
//
// Quaternion implementation of the 'DCM filter' .Incorporates the magnetic distortion
// compensation algorithms from my filter which eliminates the need for a reference
// direction of flux (bx bz) to be predefined and limits the effect of magnetic distortions to yaw
// axis only.
//
// User must define 'halfT' as the (sample period / 2), and the filter gains 'Kp' and 'Ki'.
//
// Global variables 'q0', 'q1', 'q2', 'q3' are the quaternion elements representing the estimated
// orientation.See my report for an overview of the use of quaternions in this application.
//
// User must call 'AHRSupdate()' every sample period and parse calibrated gyroscope ('gx', 'gy', 'gz'),
// accelerometer ('ax', 'ay', 'ay') and magnetometer ('mx', 'my', 'mz') data.Gyroscope units are
// radians/second, accelerometer and magnetometer units are irrelevant as the vector is normalised.
//
#include "stm32f10x.h"
#include "AHRS.h"
#include "Positioning.h"
#include <math.h>
#include <stdio.h>
/* Private define ------------------------------------------------------------*/
#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
#define halfT 0.0025f // half the sample period : 0.005s/2=0.0025s
#define ACCEL_1G 1000 //the acceleration of gravity is: 1000 mg
/* Private variables ---------------------------------------------------------*/
static float q0 = 1, q1 = 0, q2 = 0, q3 = 0; // quaternion elements representing the estimated orientation
static float exInt = 0, eyInt = 0, ezInt = 0; // scaled integral error
/* Public variables ----------------------------------------------------------*/
EulerAngle_Type EulerAngle; //unit: radian
u8 InitEulerAngle_Finished = 0;
float Magnetoresistor_mGauss_X = 0, Magnetoresistor_mGauss_Y = 0, Magnetoresistor_mGauss_Z = 0;//unit: milli-Gauss
float Accelerate_mg_X, Accelerate_mg_Y, Accelerate_mg_Z;//unit: mg
float AngularRate_dps_X, AngularRate_dps_Y, AngularRate_dps_Z;//unit: dps: degree per second
int16_t Magnetoresistor_X, Magnetoresistor_Y, Magnetoresistor_Z;
uint16_t Accelerate_X = 0, Accelerate_Y = 0, Accelerate_Z = 0;
uint16_t AngularRate_X = 0, AngularRate_Y = 0, AngularRate_Z = 0;
u8 Quaternion_Calibration_ok = 0;
/* Private macro -------------------------------------------------------------*/
/* Private typedef -----------------------------------------------------------*/
/* Private function prototypes -----------------------------------------------*/
/*******************************************************************************
* Function Name: AHRSupdate
* Description : None
* Input : None
* Output : None
* Return : None
*******************************************************************************/
void AHRSupdate(float gx, float gy, float gz, float ax, float ay, float az, float mx, float my, float mz) {
float norm;
float hx, hy, hz, bx, bz;
float vx, vy, vz, wx, wy, wz;
float ex, ey, ez;
// auxiliary variables to reduce number of repeated operations
float q0q0 = q0*q0;
float q0q1 = q0*q1;
float q0q2 = q0*q2;
float q0q3 = q0*q3;
float q1q1 = q1*q1;
float q1q2 = q1*q2;
float q1q3 = q1*q3;
float q2q2 = q2*q2;
float q2q3 = q2*q3;
float q3q3 = q3*q3;
// normalise the measurements
norm = sqrt(ax*ax + ay*ay + az*az);
ax = ax / norm;
ay = ay / norm;
az = az / norm;
norm = sqrt(mx*mx + my*my + mz*mz);
mx = mx / norm;
my = my / norm;
mz = mz / norm;
// compute reference direction of flux
hx = 2*mx*(0.5 - q2q2 - q3q3) + 2*my*(q1q2 - q0q3) + 2*mz*(q1q3 + q0q2);
hy = 2*mx*(q1q2 + q0q3) + 2*my*(0.5 - q1q1 - q3q3) + 2*mz*(q2q3 - q0q1);
hz = 2*mx*(q1q3 - q0q2) + 2*my*(q2q3 + q0q1) + 2*mz*(0.5 - q1q1 - q2q2);
bx = sqrt((hx*hx) + (hy*hy));
bz = hz;
// estimated direction of gravity and flux (v and w)
vx = 2*(q1q3 - q0q2);
vy = 2*(q0q1 + q2q3);
vz = q0q0 - q1q1 - q2q2 + q3q3;
wx = 2*bx*(0.5 - q2q2 - q3q3) + 2*bz*(q1q3 - q0q2);
wy = 2*bx*(q1q2 - q0q3) + 2*bz*(q0q1 + q2q3);
wz = 2*bx*(q0q2 + q1q3) + 2*bz*(0.5 - q1q1 - q2q2);
// error is sum of cross product between reference direction of fields and direction measured by sensors
ex = (ay*vz - az*vy) + (my*wz - mz*wy);
ey = (az*vx - ax*vz) + (mz*wx - mx*wz);
ez = (ax*vy - ay*vx) + (mx*wy - my*wx);
// integral error scaled integral gain
exInt = exInt + ex*Ki;
eyInt = eyInt + ey*Ki;
ezInt = ezInt + ez*Ki;
// adjusted gyroscope measurements
gx = gx + Kp*ex + exInt;
gy = gy + Kp*ey + eyInt;
gz = gz + Kp*ez + ezInt;
// integrate quaternion rate and normalise
q0 = q0 + (-q1*gx - q2*gy - q3*gz)*halfT;
q1 = q1 + (q0*gx + q2*gz - q3*gy)*halfT;
q2 = q2 + (q0*gy - q1*gz + q3*gx)*halfT;
q3 = q3 + (q0*gz + q1*gy - q2*gx)*halfT;
// normalise quaternion
norm = sqrt(q0*q0 + q1*q1 + q2*q2 + q3*q3);
q0 = q0 / norm;
q1 = q1 / norm;
q2 = q2 / norm;
q3 = q3 / norm;
}
float clamp(float Value, float Min, float Max)
{
if(Value > Max)
{
return Max;
}else if(Value < Min)
{
return Min;
}else
{
return Value;
}
} asha 发表于 2013-5-3 19:18 static/image/common/back.gif
你选的这几个传感器根本不行，就算用ADIS16355也达不到你的要求。我给你个数据，现在国内首家100W左右的 ...
这个数据可太感谢了，我最近做的我头都大了，加速度和角速度出来数据漂移的太厉害了，我加了kalman，反应就比较慢，STM32F103的cpu浮点运算完全不行，蛋疼的很。我现在大概知道位置解算出来能到多少精度了，再次感谢哈。 zb000000 发表于 2013-5-3 20:12 static/image/common/back.gif
这个数据可太感谢了，我最近做的我头都大了，加速度和角速度出来数据漂移的太厉害了，我加了kalman，反应 ...
10秒1米可能有戏。低档的MEMS陀螺，低动态问题不大，高动态的时候效果蛋疼。另外加气压计能明显减小偏差
LZ最好做一下零速修正，也就是假设移动速度为0，对系统建模，卡尔曼估计陀螺和加速度计零点。这样能得到比较好的精度。
加了零速修正，短时间精度能极大的提高。 asha 发表于 2013-5-3 19:18 static/image/common/back.gif
你选的这几个传感器根本不行，就算用ADIS16355也达不到你的要求。我给你个数据，现在国内首家100W左右的 ...
您好，能留下您的联系方式吗，qq或手机都可以的！我的邮箱是：1102195634@qq.com monkeynav 发表于 2013-5-3 21:39 static/image/common/back.gif
10秒1米可能有戏。低档的MEMS陀螺，低动态问题不大，高动态的时候效果蛋疼。另外加气压计能明显减小偏差
...
1.正考虑把气压计加入来修正加速度计算离地平面的高度。
2.疑问：”对系统建模，卡尔曼估计陀螺和加速度计零点“，请问这个如何实现呢？我这一块是盲点 zb000000 发表于 2013-5-3 21:46 static/image/common/back.gif
您好，能留下您的联系方式吗，qq或手机都可以的！我的邮箱是：
1487494416 monkeynav 发表于 2013-5-3 21:39 static/image/common/back.gif
10秒1米可能有戏。低档的MEMS陀螺，低动态问题不大，高动态的时候效果蛋疼。另外加气压计能明显减小偏差
...
车载的用零速修正还行，飞行器没戏。 nnnkey 发表于 2013-5-3 18:00 static/image/common/back.gif
楼主，你的这个卡尔曼怎么用的，我在程序里调用返回值怎么恒定为1呢
嗯，是的，单次滤波用的。如果要对3个参数进行单独滤波，则需要分别定义每个参数的卡尔曼参数变量！同样的卡尔曼参数则可以公用。然后分别做3个卡尔曼更新函数，里面的控制参数采用相应的控制参数（我没有权限发消息就在这儿回复哈） 可不可以扩展卡尔曼参数的维数，不如把length改为3，每个参数变为三维的，这样行不行，我试的结果是错误的，算出来没有结果，现在不知道问题出在哪里 zb000000 发表于 2013-5-4 11:24 static/image/common/back.gif
嗯，是的，单次滤波用的。如果要对3个参数进行单独滤波，则需要分别定义每个参数的卡尔曼参数变量！同样 ...
说个我的见解，你上传的那个卡尔曼如果维数大点用不了几分钟可能就发散了。 nnnkey 发表于 2013-5-4 11:43 static/image/common/back.gif
可不可以扩展卡尔曼参数的维数，不如把length改为3，每个参数变为三维的，这样行不行，我试的结果是错误的 ...
那控制参数的维数也得变，比较麻烦 asha 发表于 2013-5-4 11:59 static/image/common/back.gif
说个我的见解，你上传的那个卡尔曼如果维数大点用不了几分钟可能就发散了。...
嗯，只用于单维运算，效果还可以，主要是对传感器出来的数据直接进行滤波，不是为了传感器数据融合 zb000000 发表于 2013-5-4 12:13 static/image/common/back.gif
嗯，只用于单维运算，效果还可以，主要是对传感器出来的数据直接进行滤波，不是为了传感器数据融合 ...
恩，差不多够用了。 nnnkey 发表于 2013-5-4 11:43 static/image/common/back.gif
可不可以扩展卡尔曼参数的维数，不如把length改为3，每个参数变为三维的，这样行不行，我试的结果是错误的 ...
状态方程不支持， 本帖最后由 zb000000 于 2013-5-5 22:18 编辑
asha 发表于 2013-5-4 17:42 static/image/common/back.gif
状态方程不支持，
我发现，传感器数据经过卡尔曼滤波后，再用AHRS程序互补滤波进行姿态解算，精度非常高的。见图
图中：磁阻X->欧拉角的翻滚角； 磁阻Y->欧拉角的俯仰角；
静止时的角度值：如图，可知角度静止的精度为：0.1度。（图中欧拉角的单位：1°/100=0.01°，即角度均放大了100倍）
运动过程中的欧拉角值，角度值也相对平稳。（图中欧拉角的单位：1°/100=0.01°，即角度均放大了100倍） zb000000 发表于 2013-5-5 22:16 static/image/common/back.gif
我发现，传感器数据经过卡尔曼滤波后，再用AHRS程序互补滤波进行姿态解算，精度非常高的。见图
图中：磁 ...
小动态问题不大，就怕大动态。 楼主有没有大动态范围试试阿 zb000000 发表于 2013-5-4 11:24 static/image/common/back.gif
嗯，是的，单次滤波用的。如果要对3个参数进行单独滤波，则需要分别定义每个参数的卡尔曼参数变量！同样 ...
楼主，请问下，使用AHRS，不初始化四元数，会对后面的解算造成问题吗？
用惯导来计算经纬度 ，这个不现实吧。。。 顶楼主，顺便学习 顶楼主，好帖留名 好贴留名，早日实现 我现在零速的情况下，10秒都漂移0.5m....纠结。。。不知道楼主做到哪一步了？还有用什么传感器？ 感谢分享，但是一直没有搞太明白卡尔曼滤波的具体实现步骤，特别是参数的确定，P，Q,R 楼主！你真是救命恩人啊！我是在校学生，在做这个的课题，也是stm32处理器上做的，能分享下你的位置代码吗？我现在快愁死了！呜呜。。。 楼主！你真是救命恩人啊！我是在校学生，在做这个的课题，也是stm32处理器上做的，能分享下你的位置代码吗？我现在快愁死了！呜呜。。。 不懂，先看看。
本帖最后由 g921002 于 2014-5-8 13:24 编辑
基於樓主的代碼跟用的組件，基本上做出來的結果基本上是垃圾。
有理想是好事，但是不要無視理論基礎。
真是好东西啊 楼主算法稳定了吗，可否借鉴下QQ2252774555，谢了 刚要弄这个 感觉不错 学习了 刚要弄这个 感觉不错 学习了 谢谢分享，帮朋友下了一个{:handshake:} 很好的kalman算法
页:
[1]