//
// Created by villuton on 08.11.2024.
//
#include "KalmanFilter.h"
void SimpleKalmanInit(tSimpleKalman *env, float k){
    env->k = k;
    env->result = (float)0.0;
    env->old = (float)0.0;
}

float SimpleKalman(tSimpleKalman *env,float current){
    env->old = env->result;
    env->result = env->k  * current +(1-env->k)*env->old;
    return env->result;
}

void SimpleKalmanVect3Init(tSimpleKalmanVect3 *env, float k){
    SimpleKalmanInit(&env->x,k);
    SimpleKalmanInit(&env->y,k);
    SimpleKalmanInit(&env->z,k);
}
vector3 SimpleKalmanVect3(tSimpleKalmanVect3 *env,vector3 current){
    vector3 result = {
            SimpleKalman(&env->x,current.x),
            SimpleKalman(&env->y,current.y),
            SimpleKalman(&env->z,current.z)
    };
    return result;
}