四轴飞行器&角度融合波形 - 智能车制作

// Definitions
#define Kp 6.0f // proportional gain governs rate of convergence to accelerometer/magnetometer
#define Ki 0.03f // integral gain governs rate of convergence of gyroscope biases
#define halfT 0.0014 // half the sample period
//---------------------------------------------------------------------------------------------------
// Variable definitions
float q0 = 1, q1 = 0, q2 = 0, q3 = 0; // quaternion elements representing the estimated orientation
float exInt = 0, eyInt = 0, ezInt = 0; // scaled integral error
//====================================================================================================
// Function
//====================================================================================================
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 Q_angle=0.001,
Q_gyro=0.005,
R_angle=0.5,
dt=0.0055,//注意：dt的取值为kalman滤波器采样时间;
angle,
angle_dot;
float P[2][2] = { { 1, 0 },
{ 0, 1 }
}; //alpha_k
float Pdot[4] ={0,0,0,0}; //Pdot = (u_k-bias)
const char C_0 = 1; //H = [1 0]矩阵，用于提取
float q_bias , //陀螺仪常值偏差bias
angle_err, //观测值减去估计值
PCt_0, PCt_1, //计算K的中间步骤，分子 PCT = PH_T
E, //计算K的中间步骤，分母 E =(HPH_T + R)
K_0, K_1, //K为卡尔曼增益,K = P(K),矩阵
t_0, t_1; //更新alpha的中间步骤
//============================================================================
//函数名称：Kalman_Filter
//功能概要：卡尔曼滤波
void Kalman_Filter(float angle_m,float gyro_m) //gyro_m:gyro_measure
{
// 卡尔曼滤波开始
angle+=(gyro_m-q_bias) * dt;//先验估计
Pdot[0]=Q_angle - P[0][1] - P[1][0];// Pk-' 先验估计误差协方差的微分
Pdot[1]=- P[1][1];
Pdot[2]=- P[1][1];
Pdot[3]=Q_gyro; //Pdot = (u_k-bias)
P[0][0] += Pdot[0] * dt; // Pk- 先验估计误差协方差微分的积分 = 先验估计误差协方差
P[0][1] += Pdot[1] * dt;
P[1][0] += Pdot[2] * dt;
P[1][1] += Pdot[3] * dt; //alpha_k = alpha_k-1 + [(u_k-bias)*dt]
angle_err = angle_m - angle; //zk-先验估计观测值减去估计值
PCt_0 = C_0 * P[0][0];
PCt_1 = C_0 * P[1][0]; //计算K的中间步骤，分子 PCT = PH^T
E = R_angle + C_0 * PCt_0; //计算K的中间步骤，分母 E =(HPH^T + R)
K_0 = PCt_0 / E; //K = PH^T / (HPH^T + R)
K_1 = PCt_1 / E;
t_0 = PCt_0; //更新alpha的中间步骤
t_1 = C_0 * P[0][1];
P[0][0] -= K_0 * t_0; //后验估计误差协方差
P[0][1] -= K_0 * t_1;
P[1][0] -= K_1 * t_0; //alpha_k+1 = alpha_k - KH*alpha_k
P[1][1] -= K_1 * t_1;
angle+= K_0 * angle_err;//后验估计更新估计值作为最终结果
q_bias+= K_1 * angle_err;//后验估计更新bias的值
angle_dot=gyro_m-q_bias;//输出值（后验估计）的微分 = 角速度
// 卡尔曼滤波结束
}

复制代码