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
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
| #include <time.h>
#include <stdio.h>
#include <stdlib.h>
#include <math.h>
#define n 40
void input_func(float *array, float *array2)
{
int i, sign;
float start_dist = 10.0;
float start_vel = 2.0;
srand(time(NULL));
for(i = 0; i < 40; i++)
{
sign = rand() % 2;
if (sign == 1)
{
array[i] = start_dist + start_vel * i + ((float)rand() / (float)RAND_MAX + 0.1) * (start_dist / 100 );
array2[i] = start_vel + ((float)rand() / (float)RAND_MAX + 0.1) * (start_vel / 100 * 5);
}
else
{
array[i] = start_dist + start_vel * i - ((float)rand() / (float)RAND_MAX + 0.1) * (start_dist / 100 * 5);
array2[i] = start_vel - ((float)rand() / (float)RAND_MAX + 0.1) * (start_vel / 100 * 5);
}
}
}
void fekp_2(float *input_dist, float *input_vel, float *output_dist, float *output_vel, int input_size, float a_k, float b_k)
{
int i;
float T_obz;
T_obz = 1.0;
output_dist[0] = input_dist[0];
output_vel[0] = input_vel[0];
for(i = 1; i < input_size; i++)
{
output_vel[i] = output_vel[i-1] + (1 - b_k) / T_obz * (input_dist[i] - output_dist[i-1] - output_vel[i-1] * T_obz);
output_dist[i] = output_dist[i-1] + output_vel[i-1] * T_obz + (1 - a_k) * (input_dist[i] - output_dist[i-1] - output_vel[i-1] * T_obz);
}
}
float skov(float *array1, float sv, int input_size)
{
int i;
float sko;
sko = 0;
//printf("mean: %f \n", mean );
for (i = 1; i < input_size; i++)
{
sko += fabsf(sv - array1[i]);
}
sko = sko / (input_size - 1) ;
return sko;
}
float skod(float *array1, float sv, float inc, int input_size)
{
int i;
float sko;
sko = 0;
//printf("mean: %f \n", mean );
for (i = 1; i < input_size; i++)
{
sv += inc;
sko += fabsf(sv - array1[i]);
}
sko = sko / (input_size - 1) ;
return sko;
}
int main()
{
int input_size,i;
float a1 = 0.5, a2 = 0.7, a3 = 0.9, b1 = 0.8, b2 = 0.95;
float input_dist[n] = {0};
float input_vel[40] = {0};
input_func(input_dist, input_vel);
input_size = sizeof(input_dist)/sizeof(input_dist[0]);
printf("------\n");
printf("a1 = 0.5, a2 = 0.7, a3 = 0.9, b1 = 0.75, b1 = 0.95 \n");
printf("------\n\n");
float output_dist1[40] = {0};
float output_vel1[40] = {0};
float output_dist2[40] = {0};
float output_vel2[40] = {0};
float output_dist3[40] = {0};
float output_vel3[40] = {0};
float output_dist4[40] = {0};
float output_vel4[40] = {0};
float output_dist5[40] = {0};
float output_vel5[40] = {0};
float output_dist6[40] = {0};
float output_vel6[40] = {0};
input_size = sizeof(input_dist)/sizeof(input_dist[0]);
fekp_2(input_dist, input_vel, output_dist1, output_vel1, input_size, a1, b1);
fekp_2(input_dist, input_vel, output_dist2, output_vel2, input_size, a2, b1);
fekp_2(input_dist, input_vel, output_dist3, output_vel3, input_size, a3, b1);
fekp_2(input_dist, input_vel, output_dist4, output_vel4, input_size, a1, b2);
fekp_2(input_dist, input_vel, output_dist5, output_vel5, input_size, a2, b2);
fekp_2(input_dist, input_vel, output_dist6, output_vel6, input_size, a3, b2);
printf("Distance\n\n");
for(i = 0; i < input_size; i++)
{
printf("inp: %f, out(a1,b1): %f, out(a2,b1): %f, out(a3,b1): %f, out(a1,b2): %f, out(a2,b2): %f, out(a3,b2): %f \n"
,input_dist[i], output_dist1[i], output_dist2[i], output_dist3[i], output_dist4[i], output_dist5[i], output_dist6[i]);
}
printf("------\n\n");
printf("Velocity\n\n");
for(i = 0; i < input_size; i++)
{
printf("inp: %f, out(a1,b1): %f, out(a2,b1): %f, out(a3,b1): %f, out(a1,b2): %f, out(a2,b2): %f, out(a3,b2): %f \n"
,input_vel[i], output_vel1[i], output_vel2[i], output_vel3[i], output_vel4[i], output_vel5[i], output_vel6[i]);
}
printf("------\n\n");
printf("Velocity SKO\n\n");
printf("inp: %f, out(a1,b1): %f, out(a2,b1): %f, out(a3,b1): %f, out(a1,b2): %f, out(a2,b2): %f, out(a3,b2): %f \n"
, skov(input_vel, 2.0, input_size) ,skov(output_vel1, 2.0, input_size),skov(output_vel2, 2.0, input_size),skov(output_vel3, 2.0, input_size)
,skov(output_vel4, 2.0, input_size),skov(output_vel5, 2.0, input_size),skov(output_vel6, 2.0, input_size));
printf("------\n\n");
printf("Distance SKO\n\n");
printf("inp: %f, out(a1,b1): %f, out(a2,b1): %f, out(a3,b1): %f, out(a1,b2): %f, out(a2,b2): %f, out(a3,b2): %f \n"
, skod(input_dist, 10.0, 2.0, input_size) ,skod(output_dist1, 10.0, 2.0, input_size),skod(output_dist2, 10.0, 2.0, input_size),
skod(output_dist3, 10.0, 2.0, input_size),skod(output_dist4, 10.0, 2.0, input_size),skod(output_dist5, 10.0, 2.0, input_size),
skod(output_dist6, 10.0, 2.0, input_size));
return 0;
} |
(
Сообщение было отмечено помощь как решение
Сообщение от помощь
