Kalman filter execution too slow

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);

[Mike Croucher]

Can you also share the file data_math.mat so we can run this on our own machines?

[Annamalai N]

Yeah. Sorry for Late Reply. You can access it from this drive link.

[KALYAN ACHARJYA]

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

[Annamalai N]

Yeah but Recursive Parameter Estimation (Phase 1) actually gets executed in a minute but kalman filter (phase 2) gets too slower. For smaller values of 1000 itself the kalman filter part takes around half an hour.

Answer 1

[nick]

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:

https://www.mathworks.com/help/symbolic/sym.double.html

Hope this helps.