#include "AltitudeKalman.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;
}