组合导航算法C语言实现：从传感器噪声到姿态解算的工程落地

简介：这是一套面向惯性导航与卫星导航学习者的组合导航算法程序，采用C语言实现，适合具备一定导航原理基础、希望深入理解组合导航工程实现的高年级本科生、研究生及工程技术人员。程序覆盖纯惯性导航解算与组合导航解算，内置传统Kalman滤波、自适应Kalman滤波和抗差Kalman滤波，并支持初始对准，包括间接粗对准与Kalman滤波精对准，可输出载体的姿态角、速度与位置等信息。资源包共23个文件，约625KB，以14个C源文件为核心，配合2个头文件、3个动态链接库、2个说明文本及可执行程序，源代码集中存放于Resource文件夹，便于阅读与二次开发。目前已有908人学习下载。通过该程序，读者可系统掌握惯导与卫星导航融合的算法流程，理解不同滤波策略在组合导航中的适用场景，并借助说明文档完成数据格式配置与软件运行，为后续算法改进与工程实践提供参考。

1. 组合导航算法程序（C语言实现）：从传感器噪声到姿态解算的落地路径

飞控板上的陀螺仪零偏随温度漂移，加速度计在机体振动时输出一片噪声，磁力计在电机电流附近直接跑飞——如果你只靠单一传感器算姿态，飞机连自稳都做不到。组合导航算法程序（C语言实现）要解决的就是这件事：把陀螺仪、加速度计、磁力计甚至 GNSS 的数据按一定权重融合起来，输出稳定可靠的姿态角、速度、位置。它适合做飞控、云台、平衡车、水下机器人、车载定位的嵌入式工程师，也适合刚学完 C 语言指针和结构体、想找一个能跑在单片机上的完整项目练手的人。核心不是把公式抄一遍，而是搞清楚每个传感器在什么频段可信、滤波器参数怎么整定、定点数运算怎么防止发散。下面按“传感器特性 → 姿态融合 → 速度位置组合 → 避坑 → 进阶验证”的顺序，把一套可复现的 C 语言实现拆开讲。

2. 传感器特性与坐标系：组合导航的输入到底长什么样

2.1 陀螺仪、加速度计、磁力计的频域分工

组合导航的第一件事不是写代码，而是搞清楚每个传感器的“可信频段”。陀螺仪测角速度，短时积分得到角度，动态响应好，但零偏会随时间积分成角度漂移，典型 MEMS 陀螺零偏稳定性在 10°/h 到 100°/h 量级，换算下来每秒漂移 0.003° 到 0.03°，一分钟就是 0.2° 到 2°。加速度计测比力，静态时能感知重力方向，长期不漂，但机体振动和运动加速度会直接污染输出，所以它只适合做低频修正。磁力计测地磁方向，能修正偏航角，但电机磁场、金属结构、电流线缆都会让它输出跳变，必须做椭球拟合校准后才能用。

常见做法是：陀螺积分负责高频姿态，加速度计和磁力计以低权重、低带宽去修正陀螺的慢漂移。这个思路就是互补滤波和 Mahony 滤波的出发点。如果你一上来就把三个传感器等权平均，动态下姿态会抖得没法看。

2.2 坐标系定义与传感器安装矩阵

代码里第一组要写死的常量就是坐标系。通常用右手系：X 前、Y 右、Z 下，或者 X 前、Y 左、Z 上，取决于你的机体定义。陀螺仪和加速度计的芯片轴不一定和机体轴对齐，需要安装矩阵做旋转。比如 MPU6050 焊在板子上旋转了 90°，你读到的 gyro_x 实际对应机体 Y 轴，这时候要在初始化阶段做轴交换和符号修正。

/* 传感器轴映射：根据实际焊接方向配置 */ typedef struct { int8_t gyro_map[3]; /* 每个机体轴对应哪个原始轴，0/1/2 */ int8_t gyro_sign[3]; /* 符号，+1 或 -1 */ int8_t accel_map[3]; int8_t accel_sign[3]; } axis_map_t; /* 默认：芯片轴与机体轴一致 */ static const axis_map_t default_map = { .gyro_map = {0, 1, 2}, .gyro_sign = {1, 1, 1}, .accel_map = {0, 1, 2}, .accel_sign= {1, 1, 1} }; void apply_axis_map(const axis_map_t *m, const float raw[3], float out[3]) { for (int i = 0; i < 3; i++) { out[i] = m->gyro_sign[i] * raw[m->gyro_map[i]]; } }

这段代码把原始三轴数据按配置映射到机体轴。gyro_map和gyro_sign必须和你的 PCB 焊接方向一致，否则姿态解算会朝错误方向旋转。参数整定时先静止放平，看加速度计输出是否接近[0, 0, g]，如果不是，先改映射再调滤波。

2.3 零偏校准与温度补偿的工程做法

陀螺零偏不是常数。上电后静止采集 500 到 1000 个样本求均值，只能消掉常温零偏。温度变化 10°C，零偏可能变化几度每秒。工程上要么用带温度输出的 IMU 做查表补偿，要么在滤波器里把零偏作为状态量估计。常见做法是在卡尔曼滤波状态里加两个陀螺零偏项，让滤波器自己收敛。如果只做互补滤波，至少要在开机时做一次静止校准，并且限制积分时间，防止零偏累积。

#define CALIB_SAMPLES 800 void gyro_calibrate(float bias[3]) { float sum[3] = {0}; for (int i = 0; i < CALIB_SAMPLES; i++) { float g[3]; imu_read_gyro(g); sum[0] += g[0]; sum[1] += g[1]; sum[2] += g[2]; delay_ms(2); } for (int i = 0; i < 3; i++) { bias[i] = sum[i] / CALIB_SAMPLES; } }

校准期间机体必须静止，采样间隔要和实际运行周期一致。CALIB_SAMPLES取 800 在 500Hz 采样下约 1.6 秒，太短均值不稳，太长用户等不及。校准后每次读陀螺都要减去bias，这一步漏掉，姿态几分钟就飘走。

3. 姿态融合的 C 语言实现：互补滤波与 Mahony 滤波怎么选

3.1 互补滤波的离散化公式与代码

互补滤波的核心就一句话：陀螺积分给高频，加速度计给低频。离散化后：

angle = alpha * (angle + gyro * dt) + (1 - alpha) * accel_angle

alpha通常取 0.95 到 0.99，对应截止频率几赫兹。accel_angle由加速度计三轴算俯仰和横滚：pitch = atan2(-ax, sqrt(ay*ay + az*az))，roll = atan2(ay, az)。偏航角需要磁力计，公式稍复杂，但思路一样。

typedef struct { float pitch; float roll; float yaw; float alpha; /* 互补系数，0.95~0.99 */ } comp_filter_t; void comp_filter_update(comp_filter_t *f, const float gyro[3], const float accel[3], float dt) { /* 加速度计解算姿态角 */ float acc_pitch = atan2f(-accel[0], sqrtf(accel[1]*accel[1] + accel[2]*accel[2])); float acc_roll = atan2f(accel[1], accel[2]); /* 陀螺积分 */ float gyro_pitch = f->pitch + gyro[1] * dt; float gyro_roll = f->roll + gyro[0] * dt; /* 互补融合 */ f->pitch = f->alpha * gyro_pitch + (1.0f - f->alpha) * acc_pitch; f->roll = f->alpha * gyro_roll + (1.0f - f->alpha) * acc_roll; }

alpha越大越信任陀螺，动态响应好但漂移修正慢；越小越信任加速度计，静态稳但振动时抖。我一般先取 0.98，在 500Hz 下对应截止频率约 1.6Hz。如果飞机振动大，降到 0.95 并给加速度计加低通。注意dt必须用实际循环周期，不能用标称值，否则积分比例错了，姿态会持续偏。

3.2 Mahony 滤波的误差反馈与参数整定

Mahony 滤波比互补滤波多了一步：用加速度计和磁力计构造误差向量，通过 PI 控制器反馈修正陀螺零偏。它的好处是零偏在线估计，不需要开机静止校准，而且三轴耦合更准确。核心代码是叉积误差和四元数更新。

typedef struct { float q[4]; /* 四元数 w,x,y,z */ float integral[3]; /* 误差积分 */ float kp; float ki; } mahony_t; void mahony_update(mahony_t *m, const float gyro[3], const float accel[3], float dt) { /* 归一化加速度计 */ float a[3] = {accel[0], accel[1], accel[2]}; float norm = sqrtf(a[0]*a[0] + a[1]*a[1] + a[2]*a[2]); if (norm < 1e-6f) return; a[0] /= norm; a[1] /= norm; a[2] /= norm; /* 由四元数推算重力方向 */ float qw = m->q[0], qx = m->q[1], qy = m->q[2], qz = m->q[3]; float vx = 2.0f*(qx*qz - qw*qy); float vy = 2.0f*(qw*qx + qy*qz); float vz = qw*qw - qx*qx - qy*qy + qz*qz; /* 叉积误差 */ float ex = a[1]*vz - a[2]*vy; float ey = a[2]*vx - a[0]*vz; float ez = a[0]*vy - a[1]*vx; /* PI 修正 */ m->integral[0] += m->ki * ex * dt; m->integral[1] += m->ki * ey * dt; m->integral[2] += m->ki * ez * dt; float gx = gyro[0] + m->kp * ex + m->integral[0]; float gy = gyro[1] + m->kp * ey + m->integral[1]; float gz = gyro[2] + m->kp * ez + m->integral[2]; /* 四元数积分 */ float half_dt = 0.5f * dt; float qw_new = qw + half_dt * (-qx*gx - qy*gy - qz*gz); float qx_new = qx + half_dt * ( qw*gx + qy*gz - qz*gy); float qy_new = qy + half_dt * ( qw*gy - qx*gz + qz*gx); float qz_new = qz + half_dt * ( qw*gz + qx*gy - qy*gx); /* 归一化 */ norm = sqrtf(qw_new*qw_new + qx_new*qx_new + qy_new*qy_new + qz_new*qz_new); m->q[0] = qw_new / norm; m->q[1] = qx_new / norm; m->q[2] = qy_new / norm; m->q[3] = qz_new / norm; }

kp决定加速度计修正的强度，ki决定零偏估计速度。经验值：kp = 0.5到2.0，ki = 0.001到0.01。kp太大，振动直接灌进姿态；ki太大，零偏估计会震荡。整定时先把ki设 0，调kp到静态稳定、动态不抖，再加ki消除慢漂。磁力计参与时，偏航误差要单独处理，不能和重力误差混在一起，否则电机一转偏航就跳。

3.3 四元数转欧拉角与输出限幅

姿态输出给控制环时，通常要欧拉角。四元数转欧拉角公式：

void quat_to_euler(const float q[4], float *pitch, float *roll, float *yaw) { *pitch = asinf(2.0f*(q[0]*q[2] - q[3]*q[1])); *roll = atan2f(2.0f*(q[0]*q[1] + q[2]*q[3]), 1.0f - 2.0f*(q[1]*q[1] + q[2]*q[2])); *yaw = atan2f(2.0f*(q[0]*q[3] + q[1]*q[2]), 1.0f - 2.0f*(q[2]*q[2] + q[3]*q[3])); }

俯仰角在 ±90° 附近有万向锁，asinf输入要限幅到 [-1, 1]，否则 NaN 会传进控制环。偏航角输出后要做 unwrap，防止从 179° 跳到 -179° 导致控制舵机猛打。这些边界处理不做，试飞时就是血泪经验。

4. 速度与位置组合：从姿态到导航的扩展

4.1 加速度计积分求速度的漂移抑制

姿态稳了之后，把机体加速度旋转到导航系，减去重力，积分得速度，再积分得位置。但 MEMS 加速度计零偏在 0.01g 量级，积分 10 秒速度误差就到 1m/s，位置误差几十米。所以纯惯性导航几分钟就发散，必须用 GNSS 或气压计做修正。常见做法是卡尔曼滤波，状态量取位置、速度、姿态误差、陀螺零偏、加速度计零偏，观测量用 GNSS 位置速度和气压高度。

typedef struct { float pos[3]; float vel[3]; float acc_bias[3]; } ins_state_t; void ins_propagate(ins_state_t *s, const float acc_nav[3], float dt) { for (int i = 0; i < 3; i++) { float acc = acc_nav[i] - s->acc_bias[i]; s->pos[i] += s->vel[i] * dt + 0.5f * acc * dt * dt; s->vel[i] += acc * dt; } }

acc_nav是去掉重力后的导航系加速度。acc_bias由卡尔曼滤波在线估计，不能设死值。如果只用气压计修正高度，acc_bias的 Z 轴收敛慢，定高会上下晃，这时候要加阻尼。

4.2 GNSS 位置观测的卡尔曼更新

GNSS 更新频率通常 5 到 10Hz，比 IMU 慢得多。卡尔曼更新时只更新位置和速度，姿态误差通过协方差耦合间接修正。观测矩阵 H 取单位阵对应位置速度项，观测噪声 R 按 GNSS 精度设，水平 1 到 3 米，垂直 3 到 5 米。

void kalman_update_pos(kalman_t *k, const float gnss_pos[3], const float gnss_vel[3]) { float y[6]; for (int i = 0; i < 3; i++) { y[i] = gnss_pos[i] - k->x[i]; y[i + 3] = gnss_vel[i] - k->x[i + 3]; } /* S = H P H' + R，此处省略矩阵运算细节 */ /* K = P H' inv(S) */ /* x = x + K y */ /* P = (I - K H) P */ }

R 不能设太小，否则 GNSS 跳点直接拉偏位置；也不能太大，否则修正无效。我一般水平 R 取 2.0，垂直取 4.0，再根据实际模块的 HDOP 动态调整。GNSS 失锁时，R 设无穷大，滤波器只靠惯性递推，同时给位置方差设上限，防止协方差爆炸。

4.3 磁力计校准与偏航修正的注意事项

磁力计用之前必须做椭球拟合，消除硬磁和软磁干扰。硬磁是固定偏置，软磁是轴间耦合。校准方法：把设备绕三轴各转几圈，采集数据，拟合椭球，求偏置和变换矩阵。代码里做：

typedef struct { float offset[3]; float soft_iron[3][3]; } mag_calib_t; void mag_apply_calib(const mag_calib_t *c, const float raw[3], float out[3]) { float temp[3]; for (int i = 0; i < 3; i++) { temp[i] = raw[i] - c->offset[i]; } for (int i = 0; i < 3; i++) { out[i] = c->soft_iron[i][0] * temp[0] + c->soft_iron[i][1] * temp[1] + c->soft_iron[i][2] * temp[2]; } }

校准后磁力计输出模长应接近当地地磁强度，比如 0.48 Gauss。如果模长随方向变化超过 10%，说明软磁没校好，偏航会周期性跳。电机电流大时，磁力计数据直接不可信，这时候要么把磁力计远离动力线，要么在滤波器里根据油门大小动态调大磁力计噪声。

5. 组合导航程序避坑：从定点数溢出到滤波器发散的排查记录

5.1 现象：姿态角缓慢旋转，静止时也漂

原因：陀螺零偏未校准或校准后温度变化。解决：开机静止校准，并在滤波器里加零偏状态估计。如果只做互补滤波，至少每 10 分钟重新校准一次，或者用温度传感器查表补偿。

5.2 现象：振动时姿态剧烈抖动，加速度计输出毛刺

原因：加速度计带宽太高，机体振动直接灌入。解决：硬件上加低通电容，软件上给加速度计做滑动平均或二阶低通，截止频率设 20 到 50Hz。互补滤波的alpha降到 0.95 以下，Mahony 的kp降到 0.5。

5.3 现象：偏航角随油门变化，电机一转就偏

原因：磁力计受电机磁场干扰。解决：磁力计远离动力线，做软硬磁校准，滤波器里根据油门动态调大磁力计观测噪声。如果干扰无法避免，干脆放弃磁力计，只用陀螺积分偏航，靠 GNSS 航向或视觉修正。

5.4 现象：位置积分几分钟后发散，速度越来越大

原因：加速度计零偏未估计，纯惯性积分。解决：加 GNSS 或气压计做卡尔曼更新，状态里包含加速度计零偏。没有外部观测时，给速度加零速修正，检测到静止时把速度拉回零。

5.5 现象：定点数运算溢出，姿态突然跳变

原因：用 Q 格式定点数时，积分累加超出范围。解决：关键积分项用 32 位或 64 位，输出前做饱和限幅。如果 MCU 有 FPU，直接用浮点，别为了省那点周期把代码搞成黑匣子。

6. 进阶验证：用静态测试和转台数据判断滤波器是否收敛

写完代码别急着上飞机。先做静态测试：设备静止放平，采集 10 分钟姿态输出，看俯仰和横滚是否在 ±0.5° 内，偏航漂移是否小于 1°/分钟。然后做手动转动测试：绕三轴各转 90° 再回位，看姿态能否回到初始值，误差是否在 2° 以内。有条件的话上转台，给已知角速度，对比积分角度和转台角度，标定比例因子和零偏。

/* 静态测试：计算姿态均值和标准差 */ void static_test(const float *pitch_buf, const float *roll_buf, int n) { float sum_p = 0, sum_r = 0; for (int i = 0; i < n; i++) { sum_p += pitch_buf[i]; sum_r += roll_buf[i]; } float mean_p = sum_p / n; float mean_r = sum_r / n; float var_p = 0, var_r = 0; for (int i = 0; i < n; i++) { var_p += (pitch_buf[i] - mean_p) * (pitch_buf[i] - mean_p); var_r += (roll_buf[i] - mean_r) * (roll_buf[i] - mean_r); } var_p /= n; var_r /= n; /* 标准差应小于 0.5 度 */ }

验证时重点看三个指标：静态标准差、回位误差、动态跟随延迟。静态标准差大，说明滤波器太信任加速度计或振动没滤干净；回位误差大，说明零偏没估准或积分有累积误差；动态跟随延迟大，说明alpha太大或kp太小。我一般把这三个指标做成上位机曲线，调参时盯着看，比盲调快得多。

最后说个习惯：每次改滤波器参数，先在地面跑 10 分钟静态，再手动转，最后才上动力。直接上飞机调参，翻车成本太高。组合导航算法程序（C语言实现）的难点不在公式，而在传感器特性、坐标系、参数整定和边界处理。把这几步做扎实，姿态和位置才能稳。希望帮到你。

本文还有配套的精品资源，点击获取