commit 582e2d32671dce0f0e620bc6c2c0d7ea987d2a59 Author: Villuton Date: Sat Nov 9 01:02:34 2024 +0300 fix diff --git a/Inc/KalmanFilter.h b/Inc/KalmanFilter.h new file mode 100644 index 0000000..099c155 --- /dev/null +++ b/Inc/KalmanFilter.h @@ -0,0 +1,22 @@ +// +// Created by villuton on 08.11.2024. +// + +#ifndef KALMANFILTER_H +#define KALMANFILTER_H + +#include "vector.h" + +typedef struct { + float k; + float result; + float old; +}tSimpleKalman; + +typedef struct { + tSimpleKalman x; + tSimpleKalman y; + tSimpleKalman z; +}tSimpleKalmanVect3; + +#endif //KALMANFILTER_H diff --git a/Src/KalmanFilter.c b/Src/KalmanFilter.c new file mode 100644 index 0000000..9b4509a --- /dev/null +++ b/Src/KalmanFilter.c @@ -0,0 +1,31 @@ +// +// 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; +} + + diff --git a/modular.json b/modular.json new file mode 100644 index 0000000..6661722 --- /dev/null +++ b/modular.json @@ -0,0 +1,17 @@ +{ + "dep": [ + { + "type": "git", + "provider": "ficom_modules", + "repo": "BaseTypes" + } + ], + "cmake": { + "inc_dirs": [ + "Inc" + ], + "srcs": [ + "Src/**.c" + ] + } +} \ No newline at end of file