47 lines
1.1 KiB
C
47 lines
1.1 KiB
C
#include "main.h"
|
|
#include "fixed_height.h"
|
|
|
|
#include <math.h>
|
|
|
|
// 高度卡尔曼初始化
|
|
void AltKalman_Init(AltitudeKalman* kf) {
|
|
kf->z = 0.0f;
|
|
kf->v = 0.0f;
|
|
kf->P[0][0] = kf->P[1][1] = 1.0f;
|
|
kf->P[0][1] = kf->P[1][0] = 0.0f;
|
|
kf->Q_z = 0.01f;
|
|
kf->Q_v = 0.03f;
|
|
kf->R = 0.25f;
|
|
}
|
|
|
|
// 高度卡尔曼更新
|
|
void AltKalman_Update(AltitudeKalman* kf, float accel_z, float baro_alt, float dt) {
|
|
// 预测步骤
|
|
kf->z += kf->v * dt + 0.5f * accel_z * dt * dt;
|
|
kf->v += accel_z * dt;
|
|
|
|
// 更新协方差矩阵
|
|
kf->P[0][0] += dt * (dt * kf->P[1][1] + kf->P[0][1] + kf->P[1][0] + dt * kf->Q_z);
|
|
kf->P[0][1] += dt * kf->P[1][1];
|
|
kf->P[1][0] += dt * kf->P[1][1];
|
|
kf->P[1][1] += kf->Q_v * dt;
|
|
|
|
// 测量更新
|
|
float y = baro_alt - kf->z;
|
|
float S = kf->P[0][0] + kf->R;
|
|
float K[2] = {kf->P[0][0] / S, kf->P[1][0] / S};
|
|
|
|
// 更新状态
|
|
kf->z += K[0] * y;
|
|
kf->v += K[1] * y;
|
|
|
|
// 更新协方差
|
|
float P00_temp = kf->P[0][0];
|
|
float P01_temp = kf->P[0][1];
|
|
kf->P[0][0] -= K[0] * P00_temp;
|
|
kf->P[0][1] -= K[0] * P01_temp;
|
|
kf->P[1][0] -= K[1] * P00_temp;
|
|
kf->P[1][1] -= K[1] * P01_temp;
|
|
}
|
|
|