Getting trouble in EKF for pendulum (not tracking perfectly)

Hi everyone,

These days im trying to implement an EKF for Pendulum.

But my EKF algorithm does not track the true state smoothly.

What is my wrong ? Can any one help ?

Thanks

[Sam Chak]

Hi @Muhammed Emin

I believe EKF stands for 'Extended Kalman Filter.' How can we verify if the formulation of EKF is correct, as shown in your code?

% Extended Kalman Filter for Nonlinear Pendulum Dynamics
clc; clear all; close all;

g           = 9.81;  % acceleration 
L           = 1.0;   % length 

theta0      = pi/4;  % initial angle
theta_dot0  = 0;     % initial angular velocity
x0          = [theta0; theta_dot0];
x_hat       = [theta0; theta_dot0];

% Time vector
t_span      = [0 10];  % 
dt          = 0.01;  % 
t           = t_span(1):dt:t_span(2);

% Simulate true state with ode45
options     = odeset('RelTol', 1e-6, 'AbsTol', 1e-6);
[~, true_state] = ode45(@(t, x) pendulumDynamics(t, x, g, L), t, x0, options);

% Add noise to measurements
measurement_noise_std = 0.1;
measurement_noise     = measurement_noise_std*randn(size(true_state, 1), 1);
measurements          = true_state(:, 1) + measurement_noise;

% EKF parameters
n = 2;
m = 1; 
Q = diag([0, 1e-3]);    % process noise 
R = 5e-3;               % measurement noise 
P_prev = [1e-6, 0;
          0, 1e-6];     % initial state 

x_hat_store = zeros(length(t), n);

% EKF implementation
for i = 2:length(t)
    % State transition jacobian matrix
    F = [1, dt; -dt*g*cos(x_hat(1))/L, 1];

    % Observation jacobian matrix
    H = [cos(x_hat(1)), 0];

    % Prediction
    x_hat_ = [x_hat(1) + x_hat(2)*dt; - dt * g * sin(x_hat(1))/L];
    z_     = observationModel(x_hat(1), L);
    P_     = F*P_prev*F' + Q;

    % Update
    S = H * P_ * H' + R;
    C = P_ * H';
    K = C / S;

    y_pre = measurements(i) - z_;
    x_hat = x_hat_ + K * y_pre;
    P_hat = (eye(n) - K * H) * P_;

    % Store the estimated state
    x_hat_store(i, :) = x_hat';
    P_prev = P_hat;  
end

figure;
% subplot(3,1,1)
plot(t, true_state(:,1),  'b',   'LineWidth', 1); hold on
% subplot(3,1,2)
% plot(t, measurements,   'r--', 'LineWidth', 1);
% subplot(3,1,3)
plot(t, x_hat_store(:,1), 'g--', 'LineWidth', 1); % Plot estimated angle
% title('True State, Measurements, and Estimated Angle');
% legend('True State', 'Measurements', 'Estimated Angle');
grid on, ylim([-1.2 1.2])   % <-- added here
title('True State, and Estimated Angle');
legend('True State', 'Estimated Angle');
xlabel('Time');
ylabel('Angle');

figure;
% subplot(2,1,2);
plot(t, true_state(:,2),  'b',   'LineWidth', 1);
hold on;
plot(t, x_hat_store(:,2), 'g--', 'LineWidth', 1);   % Plot estimated angular velocity
grid on, ylim([-3.5 3.5])   % <-- added here
title('True Angular Velocity and Estimated Angular Velocity');
legend('True Angular Velocity', 'Estimated Angular Velocity');
xlabel('Time');
ylabel('Angular Velocity');

% Nonlinear pendulum dynamics function
function dxdt = pendulumDynamics(t, x, g, L)
    theta       = x(1);
    theta_dot   = x(2);
    dxdt        = [theta_dot; 
                 - g/L*sin(theta)];
end

% Observation model
function z = observationModel(x, L)
    z = L*sin(x(1));
end

[Muhammed Emin]

Hi @Sam Chak,

Yes, EKF means Extenden Kalman Filter, and i dont know you question too, maybe you can see that i cant see or maybe i have a mistake from pendulum dynamics or basic algorithm mistake etc...

Answer 1

[Paul]

The state prediction equation needs to be modified, see below.

I didn't verify the rest of the equations.

g           = 9.81;  % acceleration 
L           = 1.0;   % length 
theta0      = pi/4;  % initial angle
theta_dot0  = 0;     % initial angular velocity
x0          = [theta0; theta_dot0];
x_hat       = [theta0; theta_dot0];
% Time vector
t_span      = [0 10];  % 
dt          = 0.01;  % 
t           = t_span(1):dt:t_span(2);
% Simulate true state with ode45
options     = odeset('RelTol', 1e-6, 'AbsTol', 1e-6);
[~, true_state] = ode45(@(t, x) pendulumDynamics(t, x, g, L), t, x0, options);
% Add noise to measurements
measurement_noise_std = 0.1;
measurement_noise     = measurement_noise_std*randn(size(true_state, 1), 1);
measurements          = true_state(:, 1) + measurement_noise;
% EKF parameters
n = 2;
m = 1; 
Q = diag([0, 1e-3]);    % process noise 
R = 5e-3;               % measurement noise 
P_prev = [1e-6, 0;
          0, 1e-6];     % initial state 
x_hat_store = zeros(length(t), n);
% EKF implementation
for i = 2:length(t)
    % State transition jacobian matrix
    F = [1, dt; -dt*g*cos(x_hat(1))/L, 1];
    % Observation jacobian matrix
    H = [cos(x_hat(1)), 0];
    % Prediction
%    x_hat_ = [x_hat(1) + x_hat(2)*dt; - dt * g * sin(x_hat(1))/L];
    x_hat_ = [x_hat(1) + x_hat(2)*dt; x_hat(2) - dt * g * sin(x_hat(1))/L];
%    x_hat_ = x_hat + dt*pendulumDynamics(nan,x_hat,g,L) % alternative method
    z_     = observationModel(x_hat(1), L);
    P_     = F*P_prev*F' + Q;
    % Update
    S = H * P_ * H' + R;
    C = P_ * H';
    K = C / S;
    y_pre = measurements(i) - z_;
    x_hat = x_hat_ + K * y_pre;
    P_hat = (eye(n) - K * H) * P_;
    % Store the estimated state
    x_hat_store(i, :) = x_hat';
    P_prev = P_hat;  
end
figure;
% subplot(3,1,1)
plot(t, true_state(:,1),  'b',   'LineWidth', 1); hold on
% subplot(3,1,2)
% plot(t, measurements,   'r--', 'LineWidth', 1);
% subplot(3,1,3)
plot(t, x_hat_store(:,1), 'g--', 'LineWidth', 1); % Plot estimated angle
% title('True State, Measurements, and Estimated Angle');
% legend('True State', 'Measurements', 'Estimated Angle');
grid on, ylim([-1.2 1.2])   % <-- added here
title('True State, and Estimated Angle');
legend('True State', 'Estimated Angle');
xlabel('Time');
ylabel('Angle');

figure;
% subplot(2,1,2);
plot(t, true_state(:,2),  'b',   'LineWidth', 1);
hold on;
plot(t, x_hat_store(:,2), 'g--', 'LineWidth', 1);   % Plot estimated angular velocity
grid on, ylim([-3.5 3.5])   % <-- added here
title('True Angular Velocity and Estimated Angular Velocity');
legend('True Angular Velocity', 'Estimated Angular Velocity');
xlabel('Time');
ylabel('Angular Velocity');

% Nonlinear pendulum dynamics function
function dxdt = pendulumDynamics(t, x, g, L)
    theta       = x(1);
    theta_dot   = x(2);
    dxdt        = [theta_dot; 
                 - g/L*sin(theta)];
end
% Observation model
function z = observationModel(x, L)
    z = L*sin(x(1));
end

[Muhammed Emin]

Thanks For answering @Paul but i think you are wrong,

because our predict x_hat equation is x_hat = F*x

F matrix is discrete state space A matrix and our F matrix equal to [1,dt ; -g/L*cos(theta)*dt,1]

x matrix is our state matrix and equal to x = [theta;theta_dot];

and we multiply this two matrix we get "x_hat_ = [x_hat(1) + x_hat(2)*dt; x_hat(2) - dt * g * sin(x_hat(1))/L]" this equation.

So i think my state equation is true and also you can see tracking its still not good enough.

Thanks for answering again

[Paul]

Except what you wrote for what the state prediction should be is different that what it actually is.

Here's what the prediction equatoin should as in your comment above:

we get "x_hat_ = [x_hat(1) + x_hat(2)*dt; x_hat(2) - dt * g * sin(x_hat(1))/L]"

Here is the prediction equation from your original code line 46 as it actualy is:

x_hat_ = [x_hat(1) + x_hat(2)*dt; - dt * g * sin(x_hat(1))/L];

Do you see the difference between line 46 and what was intended? The second term is missing the x_hat(2).

Here is the corrected line from my Answer:

x_hat_ = [x_hat(1) + x_hat(2)*dt; x_hat(2) - dt * g * sin(x_hat(1))/L];

which is exactly the same as what you wrote above.

Or, why not change line 46 to be: x_hat_ = F*xhat (or use the line that I had commented as "alternative method" that is functionally equivalent.

You can see that with my correction that the estimation of the angular velocity is much better. What results are you expecting?

dbtype EKF.m 44:49

44    
45        % Prediction
46        x_hat_ = [x_hat(1) + x_hat(2) * dt; - dt * g * sin(x_hat(1))/L];
47        z_ = observationModel(x_hat(1), L);
48        P_ = F * P_prev * F' + Q;
49    

[Muhammed Emin]

Oh, I'm sorry, i just thought that i wrote prediction equation as you said, apologize for that,

But if you uncomment noisy measurement plot you can see the measurment is still good than EKF, so I still think there is a mistake in the algorithm. Because EKF must better than noisy measurement.

Is there any chance to improve my EKF algorithm to track better ?

[Paul]

Looking further at the equations ....

The measurement equation is:

y = x(1) + measurement_noise.

Given that equation, I don't believe the equations for H or z_ are correct.

H should be the Jacobian of y wrt x

z_ should be the noise-free predicted measurement based on x_hat_, which is not how z_ is defined in the code. What is observationModel supposed to be and why isn't the input to it x_hat_ ?

In addition ...

Why does F(2,1) have a cos() ? There is no cos() in the equations in pendulumDynamics.