Getting trouble in EKF for pendulum (not tracking perfectly)

Question

Muhammed Emin 2023-11-28

0
链接

此问题的直接链接

https://ww2.mathworks.cn/matlabcentral/answers/2053117-getting-trouble-in-ekf-for-pendulum-not-tracking-perfectly

评论： Paul 2023-12-1

EKF.m

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 个评论
显示无隐藏无

Sam Chak 2023-11-29

在 MATLAB Online 中打开

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 2023-11-30

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 2023-11-30

0
链接

此回答的直接链接

https://ww2.mathworks.cn/matlabcentral/answers/2053117-getting-trouble-in-ekf-for-pendulum-not-tracking-perfectly#answer_1362712

在 MATLAB Online 中打开

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 个评论
显示 2更早的评论隐藏 2更早的评论

Paul 2023-11-30

在 MATLAB Online 中打开

EKF.m

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

Muhammed Emin 2023-12-1

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 2023-12-1

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.

请先登录，再进行评论。