32 lines
783 B
C
32 lines
783 B
C
//
|
|
// 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;
|
|
}
|
|
|
|
|