KalmanFilter.cpp

#include "KalmanFilter.h"

namespace {

// ---------------------------------------------------------------------
// Bu anonim ad alani (anonymous namespace), Kalman Filtresi denklemlerini
// (predict/update) okunakli tutmak icin gereken kucuk, sabit boyutlu
// matris islemlerini (carpim, toplam, transpoze, 2x2 tersini alma)
// icerir. Genel amacli bir lineer cebir kutuphanesi degildir; sadece bu
// filtrenin 4x4 / 2x2 / 2x4 boyutlarina ozeldir.
// ---------------------------------------------------------------------

using Matrix4x4 = KalmanFilter::Matrix4x4;
using Matrix2x4 = KalmanFilter::Matrix2x4;
using Matrix4x2 = KalmanFilter::Matrix4x2;
using Matrix2x2 = KalmanFilter::Matrix2x2;
using Vector4 = KalmanFilter::Vector4;
using Vector2 = KalmanFilter::Vector2;

Matrix4x4 multiply(const Matrix4x4& A, const Matrix4x4& B) {
    Matrix4x4 result{};
    for (int i = 0; i < 4; ++i)
        for (int j = 0; j < 4; ++j) {
            double sum = 0.0;
            for (int k = 0; k < 4; ++k) sum += A[i][k] * B[k][j];
            result[i][j] = sum;
        }
    return result;
}

Vector4 multiply(const Matrix4x4& A, const Vector4& v) {
    Vector4 result{};
    for (int i = 0; i < 4; ++i) {
        double sum = 0.0;
        for (int k = 0; k < 4; ++k) sum += A[i][k] * v[k];
        result[i] = sum;
    }
    return result;
}

Matrix4x4 transpose(const Matrix4x4& A) {
    Matrix4x4 result{};
    for (int i = 0; i < 4; ++i)
        for (int j = 0; j < 4; ++j) result[j][i] = A[i][j];
    return result;
}

Matrix4x4 add(const Matrix4x4& A, const Matrix4x4& B) {
    Matrix4x4 result{};
    for (int i = 0; i < 4; ++i)
        for (int j = 0; j < 4; ++j) result[i][j] = A[i][j] + B[i][j];
    return result;
}

Matrix4x2 transposeH(const Matrix2x4& H) {
    Matrix4x2 result{};
    for (int i = 0; i < 2; ++i)
        for (int j = 0; j < 4; ++j) result[j][i] = H[i][j];
    return result;
}

// H (2x4) * P (4x4) -> (2x4)
Matrix2x4 multiplyHP(const Matrix2x4& H, const Matrix4x4& P) {
    Matrix2x4 result{};
    for (int i = 0; i < 2; ++i)
        for (int j = 0; j < 4; ++j) {
            double sum = 0.0;
            for (int k = 0; k < 4; ++k) sum += H[i][k] * P[k][j];
            result[i][j] = sum;
        }
    return result;
}

// (H*P) (2x4) * H^T (4x2) -> (2x2)
Matrix2x2 multiplyHP_Ht(const Matrix2x4& HP, const Matrix4x2& Ht) {
    Matrix2x2 result{};
    for (int i = 0; i < 2; ++i)
        for (int j = 0; j < 2; ++j) {
            double sum = 0.0;
            for (int k = 0; k < 4; ++k) sum += HP[i][k] * Ht[k][j];
            result[i][j] = sum;
        }
    return result;
}

Matrix2x2 add(const Matrix2x2& A, const Matrix2x2& B) {
    return {{{A[0][0] + B[0][0], A[0][1] + B[0][1]},
             {A[1][0] + B[1][0], A[1][1] + B[1][1]}}};
}

Matrix2x2 invert2x2(const Matrix2x2& A) {
    const double det = A[0][0] * A[1][1] - A[0][1] * A[1][0];
    const double invDet = 1.0 / det; // V1: det'in numerik olarak 0 olmadigi varsayilir (R pozitif tanimli).
    Matrix2x2 result{};
    result[0][0] = A[1][1] * invDet;
    result[0][1] = -A[0][1] * invDet;
    result[1][0] = -A[1][0] * invDet;
    result[1][1] = A[0][0] * invDet;
    return result;
}

// P (4x4) * H^T (4x2) -> (4x2)
Matrix4x2 multiplyP_Ht(const Matrix4x4& P, const Matrix4x2& Ht) {
    Matrix4x2 result{};
    for (int i = 0; i < 4; ++i)
        for (int j = 0; j < 2; ++j) {
            double sum = 0.0;
            for (int k = 0; k < 4; ++k) sum += P[i][k] * Ht[k][j];
            result[i][j] = sum;
        }
    return result;
}

// (4x2) * (2x2) -> (4x2)  [Kalman kazanci K = P*H^T * S^-1]
Matrix4x2 multiply(const Matrix4x2& A, const Matrix2x2& B) {
    Matrix4x2 result{};
    for (int i = 0; i < 4; ++i)
        for (int j = 0; j < 2; ++j) {
            double sum = 0.0;
            for (int k = 0; k < 2; ++k) sum += A[i][k] * B[k][j];
            result[i][j] = sum;
        }
    return result;
}

// K (4x2) * y (2x1) -> (4x1)
Vector4 multiply(const Matrix4x2& K, const Vector2& y) {
    Vector4 result{};
    for (int i = 0; i < 4; ++i)
        result[i] = K[i][0] * y[0] + K[i][1] * y[1];
    return result;
}

// K (4x2) * H (2x4) -> (4x4)
Matrix4x4 multiply(const Matrix4x2& K, const Matrix2x4& H) {
    Matrix4x4 result{};
    for (int i = 0; i < 4; ++i)
        for (int j = 0; j < 4; ++j) {
            double sum = 0.0;
            for (int k = 0; k < 2; ++k) sum += K[i][k] * H[k][j];
            result[i][j] = sum;
        }
    return result;
}

Matrix4x4 identity4x4() {
    Matrix4x4 I{};
    for (int i = 0; i < 4; ++i) I[i][i] = 1.0;
    return I;
}

Matrix4x4 subtract(const Matrix4x4& A, const Matrix4x4& B) {
    Matrix4x4 result{};
    for (int i = 0; i < 4; ++i)
        for (int j = 0; j < 4; ++j) result[i][j] = A[i][j] - B[i][j];
    return result;
}

} // namespace

KalmanFilter::KalmanFilter(const Vector2D& initialPosition,
                            const Vector2D& initialVelocity,
                            double processNoiseStd,
                            double measurementNoiseStd) {
    state_ = {initialPosition.x, initialPosition.y, initialVelocity.x, initialVelocity.y};

    // Baslangic belirsizligi: konum icin orta duzey, hiz icin daha buyuk
    // (cunku baslangic hizini gercekte tam bilmiyoruz).
    P_ = {{
        {measurementNoiseStd * measurementNoiseStd, 0, 0, 0},
        {0, measurementNoiseStd * measurementNoiseStd, 0, 0},
        {0, 0, 100.0, 0},
        {0, 0, 0, 100.0}
    }};

    // Olcum matrisi: sadece (x, y) konumunu dogrudan olcuyoruz.
    H_ = {{
        {1, 0, 0, 0},
        {0, 1, 0, 0}
    }};

    const double r = measurementNoiseStd * measurementNoiseStd;
    R_ = {{{r, 0}, {0, r}}};

    // Q_, predict() icinde her cagrida dt'ye gore yeniden hesaplanir
    // (asagida buildProcessNoise ile), bu yuzden burada sifirla baslatiyoruz.
    Q_ = {};
    (void)processNoiseStd;
    processNoiseStdCache_ = processNoiseStd;
}

KalmanFilter::Matrix4x4 KalmanFilter::buildStateTransition(double dt) {
    Matrix4x4 F = identity4x4();
    F[0][2] = dt; // x += vx * dt
    F[1][3] = dt; // y += vy * dt
    return F;
}

KalmanFilter::Matrix4x4 KalmanFilter::buildProcessNoise(double dt, double processNoiseStd) {
    // Beyaz gurultulu ivme (white noise acceleration) modeline dayali
    // klasik ayrik-zamanli surec gurultusu matrisi. x ve y eksenleri
    // birbirinden bagimsiz kabul edilir (capraz terimler sifir).
    const double q = processNoiseStd * processNoiseStd;
    const double dt2 = dt * dt;
    const double dt3 = dt2 * dt;

    Matrix4x4 Q{};
    // x - vx bloğu
    Q[0][0] = dt3 / 3.0 * q;
    Q[0][2] = dt2 / 2.0 * q;
    Q[2][0] = dt2 / 2.0 * q;
    Q[2][2] = dt * q;

    // y - vy bloğu
    Q[1][1] = dt3 / 3.0 * q;
    Q[1][3] = dt2 / 2.0 * q;
    Q[3][1] = dt2 / 2.0 * q;
    Q[3][3] = dt * q;

    return Q;
}

void KalmanFilter::predict(double dt) {
    const Matrix4x4 F = buildStateTransition(dt);
    const Matrix4x4 Q = buildProcessNoise(dt, processNoiseStdCache_);

    // x = F * x
    state_ = multiply(F, state_);

    // P = F * P * F^T + Q
    const Matrix4x4 FP = multiply(F, P_);
    const Matrix4x4 Ft = transpose(F);
    P_ = add(multiply(FP, Ft), Q);
}

void KalmanFilter::update(const Vector2D& measurement) {
    const Vector2 z = {measurement.x, measurement.y};

    // Innovasyon (olcum artigi): y = z - H*x
    const Vector2 Hx = {state_[0], state_[1]}; // H * x, H sadece pozisyonu secer
    const Vector2 y = {z[0] - Hx[0], z[1] - Hx[1]};

    const Matrix4x2 Ht = transposeH(H_);
    const Matrix2x4 HP = multiplyHP(H_, P_);
    const Matrix2x2 S = add(multiplyHP_Ht(HP, Ht), R_);
    const Matrix2x2 Sinv = invert2x2(S);

    const Matrix4x2 PHt = multiplyP_Ht(P_, Ht);
    const Matrix4x2 K = multiply(PHt, Sinv); // Kalman kazanci

    // x = x + K*y
    const Vector4 correction = multiply(K, y);
    for (int i = 0; i < 4; ++i) state_[i] += correction[i];

    // P = (I - K*H) * P
    const Matrix4x4 KH = multiply(K, H_);
    const Matrix4x4 IminusKH = subtract(identity4x4(), KH);
    P_ = multiply(IminusKH, P_);
}