Kalman filter execution too slow
7 Ansichten (letzte 30 Tage)
Ältere Kommentare anzeigen
Annamalai N
am 20 Mär. 2024
Bearbeitet: Neelanshu
am 29 Apr. 2024
I have written a script for doing SOC estimation using Kalman Filter. The Recursive Parameter Estimation done before it is really faster but not the later Kalman filter. I don't know why its lot slower and 1000 iterations takes more than half an hour ! Why So ?
%Setup
clc;
clear;
D = load("data_math.mat");
%load("mar18_1.mat");
sympref('FloatingPointOutput',true);
syms R1 RC1;
C = 10.0056;
%C = 3.28; SOC becomes negative overtime ! (From IIT_data, max discharge)
%Sampled Data
%U_L = S.synthData.V; % U_L data
%I_L = -S.synthData.I; % I_L data
%Tsamp = S.synthData.t; % Sampled Time
%U_L = T.Voltage(1749:3656);
%I_L = T.Current(1749:3656);
%Tsamp = T.Time(1749:3656);
U_L = D.Datamath.Voltage;
I_L = D.Datamath.Current;
Tsamp = D.Datamath.Time;
%RLS Parameters
lamda=0.98; % forget factor
Theta = zeros(4,1); % Initial value of theta vector
P=eye(4); % Initial value of Covariance Matrix
N = length(Tsamp); % Number of samples
theta_arr = zeros(4,N-1);% For storing parameters
% Other Variables
O_CV = zeros(1,N-1);
R_O = zeros(1,N-1);
SOC_arr = zeros(1,N);
y_k_arr = zeros(1,N-1);
SOC_arr(1) = 1; % As We are considering discharging, Intially SOC will be high and let's assume it to be 1.
%psi_fn = [1 0 I_L(1) 0]; % Initial value of psi_fn (1 I_L(0) I_L(1) U_L(0))
%NOTE n=1 corresponds to t = 0, n = 2 is same as of (t=1)
% RLS for Circuit Model Parameter Estimation
for n = 2 : N
%if n != 1
psi_fn = [1 U_L(n-1) I_L(n) I_L(n-1)];
T = Tsamp(n) - Tsamp(n-1);
%end
error_rls = U_L(n) - psi_fn*Theta ; % Instantaneous error of RLS
K = P*(transpose(psi_fn))/((lamda + (psi_fn)*P*transpose(psi_fn))); % Gain vector
P= (lamda^-1)*(P - K*(psi_fn)*P); % Correlation Matrix Update
Theta = Theta + error_rls*K; % Weights Update
theta_arr(:,n-1) = Theta;
O_CV(n-1) = Theta(1)/(1-Theta(2));
R_O(n-1) = T*(Theta(4)-Theta(3))/(T+Theta(2));
SOC_arr(n) = SOC_arr(n-1) - (I_L(n) * T) /(3600*C);
y_k_arr(n-1) = (psi_fn*Theta) + error_rls;
end
% Circuit Parameters
R_O_val = R_O(N-1);
eq1 = 0 == -((T-(2*RC1))+(theta_arr(2,N-1)*(T+(2*RC1))));
%eq2 = 0 == -(((R_O_val*T)+(R1*T)+(2*R_O_val*RC1))+(theta_arr(3,n-1)*(T+(2*RC1))));
%eq3 = 0 == -((R_O_val*T)+(R1*T)-(2*R_O_val*RC1))+(theta_arr(4,n-1)*(T+(2*RC1))));
eq2 = 0 == -(((R_O_val*T)+(R1*T))+((theta_arr(3,N-1)*theta_arr(4,N-1))*(T+(2*RC1))));
S = solve(eq1,eq2);
R1_val = S.R1;
C1_val = S.RC1/S.R1 ;
%Observed Values at last: R_O = 0.0230, R1 = 2.8450, C1 = 950.5212
% Gain Scheduling/ Linearization of OCV vs SOC plot
OCV_SOC_fit = polyfit(SOC_arr(2:N),O_CV,9);
dOCV_SOC_fit = polyder(OCV_SOC_fit);
k_arr = zeros(1,10);
b_arr = zeros(1,10);
for i = 1:10
k_arr(i) = polyval(dOCV_SOC_fit,0.05*((2*i)-1));
b_arr(i) = polyval(OCV_SOC_fit,0.05*((2*i)-1)) - (k_arr(i)*((2*i)-1)*0.05);
end
% Kalman filter
% Initial values
eta = 1; % Current Efficiency
Ad = [[1-(1/(R1_val*C1_val)) 0];[0 1]];
Bd = [(1/C1_val); 1/(3600*C)];
D = R_O_val;
X_arr = zeros(2,N);
X_val = [0; 1]; %Assume initial value of V2 is unknown
P_kal = eye(2);
Q_kal = diag([0.1 0.1]);
R = 1;
%Y_k_arr = zeros(1,N);
%Y_k_arr(1) = U_L(1) - b_arr(10);
% Iterations
for n = 2:N
Cd = [1 k_arr(min(ceil(X_val(2)/0.1),10))];
Uin = [I_L(n)];
% Prediction Update
X_val = Ad*X_val + Bd*Uin;
P_kal = (Ad*P_kal*transpose(Ad));
% Kalman Gain Update
Kal_Gain = (P_kal*transpose(Cd))/((Cd*P_kal*transpose(Cd))+R);
% State Estimation Update
X_val = X_val + Kal_Gain*((U_L(n) - b_arr(min(ceil(X_val(2)/0.1),10))) - (Cd*X_val) - (D*Uin));
X_arr(:,n) = X_val;
% Error Covariance update
P_kal = (eye(2)-(Kal_Gain*Cd))*P_kal;
if mod(n,1000) == 0
disp(n);
end
end
%Plots
figure;
fsize=14; % plot text font size
%SOC_arr_rev = flip(SOC_arr,2);
plot(Tsamp(2:N),X_arr(2,2:N),'','linewidth',4);
%plot(Tsamp(2:N),y_k_arr,"r",'LineWidth',4);
%hold on;
%plot(Tsamp(2:N),U_L(2:N),"b",'LineWidth',4);
%hold off;
ylim([3 4.5]);
%ax=xticklabels;
%xticklabels(flip(ax))
xlabel('time','FontName','Times New Roman','FontSize',fsize);
ylabel('SOC','FontName','Times New Roman','FontSize',fsize);
4 Kommentare
KALYAN ACHARJYA
am 25 Mär. 2024
Bearbeitet: KALYAN ACHARJYA
am 25 Mär. 2024
It performing approximately 100,000 for loop iterations in two phases, its operation duration may vary depending on array processing. Consider experimenting with smaller values of N to check performance
Akzeptierte Antwort
Neelanshu
am 17 Apr. 2024
Bearbeitet: Neelanshu
am 29 Apr. 2024
Hi Annamalai,
The reason that Kalman filter is not fast enough is due to evaluation of the equations with symbolic variables. The matrix Ad and Bd are symbolic variables instead of double which causes 1 iteration of the Kalman filter loop to take 10 ms on my Windows 11 2.9GHz 6 core processor.
When MATLAB performs arithmetic on normal numeric values the arithmetic is performed directly on hardware -- the floating-point computation unit of your CPU core (or cores).Symbolic computations performed via a symbolic math engine.
You can convert the variables R1_val and C1_val, used in the computation of Ad and Bd, to double precision using the function "double" which will make the computation much faster, as shown in the code snippet below :
R1_val = double(R1_val)
C1_val = double(C1_val)
You may refer to the following documentation to learn more about the "double" function:
Hope this helps.
0 Kommentare
Weitere Antworten (0)
Siehe auch
Kategorien
Mehr zu State-Space Control Design and Estimation finden Sie in Help Center und File Exchange
Community Treasure Hunt
Find the treasures in MATLAB Central and discover how the community can help you!
Start Hunting!