# Getting trouble in EKF for pendulum (not tracking perfectly)

9 views (last 30 days)
Muhammed Emin on 28 Nov 2023
Commented: Paul on 1 Dec 2023
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
##### 2 CommentsShow NoneHide None
Sam Chak on 29 Nov 2023
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 on 30 Nov 2023
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...

Paul on 30 Nov 2023
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 on 1 Dec 2023
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 on 1 Dec 2023
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_ ?
Why does F(2,1) have a cos() ? There is no cos() in the equations in pendulumDynamics.