Complementary_Filtering/main.asv at main · Gmark117/Complementary_Filtering · GitHub

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
%% Main optimization to find compl filters
close all
clear
clc

addpath(genpath(pwd))

%% Complementary filtering
ft0 = [10 10];
filt_order = 5;

% High pass
% B numerator coefficient
% A denominator coeff
[Bh, Ah] = butter(filt_order, ft0(1), "high", "s");
[Bl, Al] = butter(filt_order, ft0(2),  "low", "s");

tfH = tf(Bh, Ah);
tfL = tf(Bl, Al);

%% Plot
figure()
bode(tfH, tfL, {1e-6, 1e6})
legend('High pass', 'Low pass')
grid on

%% Simulation
myobj = sim('AltBaroInertial.slx', ...
    'SrcWorkspace', 'current', ...
    'StopTime', '200');
out = myobj.yout;

%% Plot initialization results
figure();
p = plot( out(:,1), out(:,2), ...
    out(:,1), out(:,3), ...
    out(:,1), out(:,4), ...
    out(:,1), out(:,5));
p(1).LineWidth = 1.5;
p(2).LineWidth = 2;

legend('Real Height', 'Filtered Height', 'Barometric Height', 'Accelerometer Height')
title('Initialization')

%% Optimization step
ft = runOPTIM_comp_no_const(ft0)

%% Optimization output
[Bh, Ah, kh] = butter(filt_order, ft(1), "high", "s");
[Bl, Al, kl] = butter(filt_order, ft(2),  "low", "s");

tfH = zp2sos(Bh, Ah, kh);
tfL = zp2sos(Bl, Al, kl);

%tfH = tf(Bh, Ah);
%tfL = tf(Bl, Al);

n_p = 10000;
err = 1e2/n_p;
w   = logspace(0,2,n_p);

[magH,phaseH] = bode(tfH,w);
[magL,phaseL] = bode(tfL,w);

magH_db = mag2db(magH);
magL_db = mag2db(magL);

% equivalent complementary filter frequency
ft_eq_index = find(abs(magH_db-magL_db)<err);
ft_eq       = w(ft_eq_index);

%% Simulation
myobj = sim('AltBaroInertial.slx',...
    'SrcWorkspace', 'current',...
    'StopTime', '200');
out = myobj.yout;

%% Plot optimization results
figure()
bodeplot(tfH,tfL,{1e0, 1e2})
xline(ft_eq)

legend('High pass', 'Low pass')
grid on

figure();
p= plot( out(:,1), out(:,2), ...
    out(:,1), out(:,3), ...
    out(:,1), out(:,4), ...
    out(:,1), out(:,5));
p(1).LineWidth = 1.5;
p(2).LineWidth = 2;

legend ('Real Height', 'Filtered Height', 'Barometric Height', 'Accelerometer Height')
title ('Optimized value')

%% Verify if the two filters are complementary
figure()
bodeplot(tfH+tfL, {1e-6,1e6})
xline(ft_eq)