1

zhyinch

2019-12-15 bb37bc0f05735470caaf3f7e4d7eec9551bf3d44

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
#include "TrackingDiff.h"
#include <stdlib.h>
 
 
TrackingDiffClass* NewTrackingDiffClass(float vel_factor, float pos_factor, float time_factor)
{
    TrackingDiffClass* p = (TrackingDiffClass*) malloc(sizeof(TrackingDiffClass));
    
    p->pos_factor = pos_factor;
    p->vel_factor = vel_factor;
    p->time_correction = time_factor;
    
    p->pos_predict = 0;
    p->vel_predict = 0;
    p->error = 0;
    
    return p;
}
 
void TrackingDiffUpdate(TrackingDiffClass* self, float target)
{
    self->error = target - self->pos_predict;
    
    self->vel_predict += (self->error * self->vel_factor) * self->time_correction;
    self->pos_predict += (self->vel_predict + self->error * self->pos_factor) * self->time_correction;
}