请用C语言写出四元数滤波算法

以下是使用C语言实现的四元数滤波算法示例：

#include <stdio.h>
#include <math.h>

typedef struct Quaternion {
  float w;
  float x;
  float y;
  float z;
} Quaternion;

Quaternion q_normalize(Quaternion q) {
  float norm = sqrt(q.w*q.w + q.x*q.x + q.y*q.y + q.z*q.z);
  q.w /= norm;
  q.x /= norm;
  q.y /= norm;
  q.z /= norm;
  return q;
}

Quaternion q_conjugate(Quaternion q) {
  Quaternion result = {q.w, -q.x, -q.y, -q.z};
  return result;
}

Quaternion q_multiply(Quaternion q1, Quaternion q2) {
  Quaternion result = {0};
  result.w = q1.w*q2.w - q1.x*q2.x - q1.y*q2.y - q1.z*q2.z;
  result.x = q1.w*q2.x + q1.x*q2.w + q1.y*q2.z - q1.z*q2.y;
  result.y = q1.w*q2.y - q1.x*q2.z + q1.y*q2.w + q1.z*q2.x;
  result.z = q1.w*q2.z + q1.x*q2.y - q1.y*q2.x + q1.z*q2.w;
  return result;
}

Quaternion q_average(Quaternion q[], int n) {
  Quaternion result = {0};
  for (int i = 0; i < n; i++) {
    result.w += q[i].w;
    result.x += q[i].x;
    result.y += q[i].y;
    result.z += q[i].z;
  }
  result.w /= n;
  result.x /= n;
  result.y /= n;
  result.z /= n;
  return q_normalize(result);
}

int main() {
  Quaternion q[5] = {{1, 0, 0, 0}, {0.5, 0.5, 0.5, 0.5}, {0, 1, 0, 0}, {0, 0, 1, 0}, {0, 0, 0, 1}};
  Quaternion q_filtered = q_average(q, 5);
  printf("Filtered quaternion: (%f, %f, %f, %f)\n", q_filtered.w, q_filtered.x, q_filtered.y, q_filtered.z);
  return 0;
}

该示例中定义了四元数结构体Quaternion，实现了四元数归一化、共轭和乘法运算。其中，q_multiply函数实现了两个四元数的乘法，q_average函数实现了对一组四元数求平均值，并返回一个归一化后的四元数作为滤波结果。在main函数中，定义了5个四元数，使用q_average函数对其进行滤波，并输出滤波结果。