Getting trouble in EKF for pendulum (not tracking perfectly)

9 views (last 30 days)
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 Comments
Sam Chak
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
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...

Sign in to comment.

Answers (1)

Paul
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
  4 Comments
Muhammed Emin
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
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_ ?
In addition ...
Why does F(2,1) have a cos() ? There is no cos() in the equations in pendulumDynamics.

Sign in to comment.

Community Treasure Hunt

Find the treasures in MATLAB Central and discover how the community can help you!

Start Hunting!