多太阳敏感器数据融合算法详解

多太阳敏感器数据融合算法详解

1. 概述

在卫星姿态确定系统中，通常安装多个太阳敏感器（太敏）以提高测量可靠性和精度。本文详细介绍如何融合多个有效太敏数据，获得最优的太阳矢量估计，包括加权最小二乘法、递推加权平均法等算法及其C语言实现。

2. 问题描述

假设卫星上安装有$N$个太阳敏感器，第$i$个敏感器测量得到的太阳单位矢量为：

$${\mathbf{S}}_i=\left[\begin{array}{c}{x}_i\\ y_i\\ z_i\end{array}\right],i=1,2,\dots ,N$$

每个测量值都有相应的权重$w_i$，反映其测量精度。目标是融合这些测量值，得到最优的太阳矢量估计$\hat{\mathbf{S}}$。

3. 数据融合算法

3.1 加权平均法

最简单的融合方法是加权平均：

$$\hat{\mathbf{S}}=\frac{{\sum }_{i=1}^N{w}_i{\mathbf{S}}_i}{\Vert {\sum }_{i=1}^N{w}_i{\mathbf{S}}_i\Vert }$$

其中权重$w_i$可以根据测量误差方差确定。

3.2 加权最小二乘法

更精确的方法是加权最小二乘估计，目标函数为：

$$J(\hat{\mathbf{S}})=\sum _{i=1}^N{w}_i\Vert {\mathbf{S}}_i-\hat{\mathbf{S}}{\Vert }^2$$

约束条件：$\Vert \hat{\mathbf{S}}\Vert =1$

求解得到最优估计：

$$\hat{\mathbf{S}}=\frac{{\sum }_{i=1}^N{w}_i{\mathbf{S}}_i}{\Vert {\sum }_{i=1}^N{w}_i{\mathbf{S}}_i\Vert }$$

3.3 协方差加权融合

如果知道各敏感器的测量误差协方差矩阵${\mathbf{R}}_i$，可采用最优加权：

$$\hat{\mathbf{S}}={\left(\sum _{i=1}^N{\mathbf{R}}_i^{-1}\right)}^{-1}\sum _{i=1}^N{\mathbf{R}}_i^{-1}{\mathbf{S}}_i$$

然后进行单位化：

$$\hat{\mathbf{S}}=\frac{\hat{\mathbf{S}}}{\Vert \hat{\mathbf{S}}\Vert }$$

4. 权重确定方法

4.1 基于测量误差的权重

$$w_i=\frac{1}{{\sigma }_i^2}$$

其中${\sigma }_i^2$是第$i$个敏感器的测量误差方差。

4.2 基于几何关系的权重

对于数字式太阳敏感器，权重可根据太阳角度确定：

$$w_i=\cos {\theta }_i$$

其中${\theta }_i$是太阳矢量与敏感器光轴夹角。

4.3 基于数据一致性的权重

使用测量值之间的相似度确定权重：

$$w_i=\frac{1}{1+{\sum }_{j\ne i}\Vert {\mathbf{S}}_i-{\mathbf{S}}_j{\Vert }^2}$$

5. 异常值检测与处理

5.1 基于残差的异常检测

计算每个测量值的残差：

$$r_i=\Vert {\mathbf{S}}_i-\hat{\mathbf{S}}\Vert$$

如果$r_i>3\sigma$，则认为该测量值为异常值。

5.2 基于投票机制的异常检测

计算每对测量值之间的夹角：

$${\theta }_{ij}=\arccos ({\mathbf{S}}_i\cdot {\mathbf{S}}_j)$$

如果某个测量值与其他多数测量值夹角过大，则判定为异常。

6. C语言实现

#include <stdio.h>
#include <math.h>
#include <stdlib.h>
#include <string.h>#ifndef M_PI
#define M_PI 3.14159265358979323846
#endif#define MAX_SUN_SENSORS 8
#define MIN_VALID_SENSORS 2
#define OUTLIER_THRESHOLD 0.17365 // 约10度对应的余弦值typedef struct {double x;double y;double z;double weight;int is_valid;
} SunMeasurement;typedef struct {double x;double y;double z;
} Vector3D;typedef struct {SunMeasurement measurements[MAX_SUN_SENSORS];int count;int valid_count;
} SunSensorData;/*** @brief 计算矢量模长*/
double vector_norm(const Vector3D* vec)
{return sqrt(vec->x * vec->x + vec->y * vec->y + vec->z * vec->z);
}/*** @brief 矢量点积*/
double vector_dot(const Vector3D* a, const Vector3D* b)
{return a->x * b->x + a->y * b->y + a->z * b->z;
}/*** @brief 矢量单位化*/
int vector_normalize(Vector3D* vec)
{double norm = vector_norm(vec);if (norm < 1e-12) {return -1;}vec->x /= norm;vec->y /= norm;vec->z /= norm;return 0;
}/*** @brief 计算两个矢量的夹角余弦值*/
double vector_cosine(const Vector3D* a, const Vector3D* b)
{double dot_product = vector_dot(a, b);double norm_a = vector_norm(a);double norm_b = vector_norm(b);if (norm_a < 1e-12 || norm_b < 1e-12) {return 0.0;}return dot_product / (norm_a * norm_b);
}/*** @brief 基于一致性的异常值检测*/
void detect_outliers(SunSensorData* data)
{int i, j;double consistency_score[MAX_SUN_SENSORS] = {0};// 计算每个测量值与其他有效测量值的一致性for (i = 0; i < data->count; i++) {if (!data->measurements[i].is_valid) continue;Vector3D vec_i = {data->measurements[i].x,data->measurements[i].y, data->measurements[i].z};int consistent_count = 0;for (j = 0; j < data->count; j++) {if (i == j || !data->measurements[j].is_valid) continue;Vector3D vec_j = {data->measurements[j].x,data->measurements[j].y,data->measurements[j].z};double cosine = vector_cosine(&vec_i, &vec_j);if (cosine > OUTLIER_THRESHOLD) {consistent_count++;}}consistency_score[i] = (double)consistent_count / (data->valid_count - 1);// 如果与少于一半的有效测量值一致，标记为异常if (consistency_score[i] < 0.5 && data->valid_count >= 3) {data->measurements[i].is_valid = 0;data->valid_count--;printf("检测到异常值: 传感器 %d, 一致性分数: %.3f\n", i, consistency_score[i]);}}
}/*** @brief 基于测量误差确定权重*/
void calculate_weights_by_error(SunSensorData* data, const double* error_variances)
{for (int i = 0; i < data->count; i++) {if (data->measurements[i].is_valid) {// 权重与误差方差成反比data->measurements[i].weight = 1.0 / (error_variances[i] + 1e-12);}}
}/*** @brief 基于几何关系确定权重（适用于数字式太敏）*/
void calculate_weights_by_geometry(SunSensorData* data)
{for (int i = 0; i < data->count; i++) {if (data->measurements[i].is_valid) {Vector3D vec = {data->measurements[i].x,data->measurements[i].y,data->measurements[i].z};// 假设光轴为Z轴，权重与太阳矢量的Z分量绝对值成正比// 这反映了测量精度随太阳角度的变化data->measurements[i].weight = fabs(vec.z);// 确保权重不为零if (data->measurements[i].weight < 0.1) {data->measurements[i].weight = 0.1;}}}
}/*** @brief 加权最小二乘融合*/
int weighted_least_squares_fusion(const SunSensorData* data, Vector3D* fused_sun_vector)
{if (data->valid_count < MIN_VALID_SENSORS) {return -1; // 有效数据不足}Vector3D weighted_sum = {0, 0, 0};double total_weight = 0.0;for (int i = 0; i < data->count; i++) {if (data->measurements[i].is_valid) {double weight = data->measurements[i].weight;weighted_sum.x += weight * data->measurements[i].x;weighted_sum.y += weight * data->measurements[i].y;weighted_sum.z += weight * data->measurements[i].z;total_weight += weight;}}if (total_weight < 1e-12) {return -2; // 权重总和太小}// 单位化得到最终结果fused_sun_vector->x = weighted_sum.x;fused_sun_vector->y = weighted_sum.y;fused_sun_vector->z = weighted_sum.z;return vector_normalize(fused_sun_vector);
}/*** @brief 协方差加权融合（简化版）*/
int covariance_weighted_fusion(const SunSensorData* data, const double covariance_matrix[][3], Vector3D* fused_sun_vector)
{if (data->valid_count < MIN_VALID_SENSORS) {return -1;}// 简化的协方差加权：假设各传感器误差独立且各向同性Vector3D weighted_sum = {0, 0, 0};double total_inverse_variance = 0.0;for (int i = 0; i < data->count; i++) {if (data->measurements[i].is_valid) {// 使用方差倒数作为权重double inverse_variance = 1.0 / (covariance_matrix[i][0] + 1e-12);weighted_sum.x += inverse_variance * data->measurements[i].x;weighted_sum.y += inverse_variance * data->measurements[i].y;weighted_sum.z += inverse_variance * data->measurements[i].z;total_inverse_variance += inverse_variance;}}if (total_inverse_variance < 1e-12) {return -2;}// 计算加权平均fused_sun_vector->x = weighted_sum.x / total_inverse_variance;fused_sun_vector->y = weighted_sum.y / total_inverse_variance;fused_sun_vector->z = weighted_sum.z / total_inverse_variance;return vector_normalize(fused_sun_vector);
}/*** @brief 计算融合结果的精度指标*/
void calculate_fusion_metrics(const SunSensorData* data, const Vector3D* fused_vector,double* max_residual, double* rms_residual)
{double sum_sq_residual = 0.0;*max_residual = 0.0;int valid_count = 0;for (int i = 0; i < data->count; i++) {if (data->measurements[i].is_valid) {Vector3D residual_vec = {data->measurements[i].x - fused_vector->x,data->measurements[i].y - fused_vector->y,data->measurements[i].z - fused_vector->z};double residual = vector_norm(&residual_vec);sum_sq_residual += residual * residual;if (residual > *max_residual) {*max_residual = residual;}valid_count++;}}if (valid_count > 0) {*rms_residual = sqrt(sum_sq_residual / valid_count);} else {*rms_residual = 0.0;}
}/*** @brief 主融合函数*/
int fuse_sun_sensors(SunSensorData* data, Vector3D* fused_sun_vector, int use_geometry_weights, double* fusion_quality)
{// 步骤1：异常值检测if (data->valid_count >= 3) {detect_outliers(data);}// 步骤2：确定权重if (use_geometry_weights) {calculate_weights_by_geometry(data);} else {// 使用默认权重或基于误差的权重double default_weights[MAX_SUN_SENSORS];for (int i = 0; i < data->count; i++) {default_weights[i] = 1.0; // 默认等权重}calculate_weights_by_error(data, default_weights);}// 步骤3：数据融合int result = weighted_least_squares_fusion(data, fused_sun_vector);// 步骤4：计算融合质量指标if (result == 0) {double max_residual, rms_residual;calculate_fusion_metrics(data, fused_sun_vector, &max_residual, &rms_residual);*fusion_quality = 1.0 / (1.0 + rms_residual); // 质量指标：残差越小，质量越高printf("融合质量: %.3f, 最大残差: %.6f, RMS残差: %.6f\n", *fusion_quality, max_residual, rms_residual);}return result;
}// 测试示例
int main()
{SunSensorData sensor_data;Vector3D fused_vector;double fusion_quality;// 初始化测试数据（4个太阳敏感器）sensor_data.count = 4;sensor_data.valid_count = 4;// 正常测量值（模拟太阳在特定方向）sensor_data.measurements[0] = (SunMeasurement){0.7071, 0.7071, 0.0, 1.0, 1}; // 45度sensor_data.measurements[1] = (SunMeasurement){0.7000, 0.7141, 0.0, 1.0, 1}; // 带有小误差sensor_data.measurements[2] = (SunMeasurement){0.7100, 0.7042, 0.0, 1.0, 1}; // 带有小误差  sensor_data.measurements[3] = (SunMeasurement){0.5000, 0.8660, 0.0, 1.0, 1}; // 异常值（60度）printf("=== 多太阳敏感器数据融合测试 ===\n");printf("有效传感器数量: %d\n", sensor_data.valid_count);// 执行数据融合int result = fuse_sun_sensors(&sensor_data, &fused_vector, 0, &fusion_quality);if (result == 0) {printf("融合成功!\n");printf("融合后的太阳矢量: [%.6f, %.6f, %.6f]\n", fused_vector.x, fused_vector.y, fused_vector.z);printf("融合质量: %.3f\n", fusion_quality);} else {printf("融合失败，错误码: %d\n", result);}return 0;
}

7. 编译与运行

gcc -o sun_sensor_fusion sun_sensor_fusion.c -lm
./sun_sensor_fusion

8. 算法性能分析

8.1 计算复杂度

• 异常检测：$O(N^2)$
• 权重计算：$O(N)$
• 数据融合：$O(N)$

8.2 精度提升

通过多传感器融合，理论上精度可提高$\sqrt{N}$倍（假设各传感器独立同分布）。

9. 实际应用考虑

1. 实时性要求：算法需要满足卫星控制系统的实时性要求
2. 内存限制：在嵌入式系统中需要注意内存使用
3. 故障处理：需要处理传感器完全失效的情况
4. 温度补偿：考虑温度对测量精度的影响

10. 总结

本文详细介绍了多太阳敏感器数据融合的多种算法，包括加权平均法、加权最小二乘法和协方差加权融合法，并提供了完整的C语言实现。在实际工程应用中，需要根据具体传感器特性和系统要求选择合适的融合策略，并充分考虑异常值处理和实时性要求。