Kalman filter execution too slow
7 views (last 30 days)
Show older comments
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 Comments
KALYAN ACHARJYA
on 25 Mar 2024
Edited: KALYAN ACHARJYA
on 25 Mar 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
Accepted Answer
Neelanshu
on 17 Apr 2024
Edited: Neelanshu
on 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 Comments
More Answers (0)
See Also
Community Treasure Hunt
Find the treasures in MATLAB Central and discover how the community can help you!
Start Hunting!