# Kalman filter 2d

46 views (last 30 days)

Show older comments

kalmanfilterlearner
on 14 Oct 2024 at 20:59

Edited: William Rose
on 17 Oct 2024 at 5:45

##### 3 Comments

William Rose
on 16 Oct 2024 at 6:19

Thank you for attaching your code. YOu can format your code as code, and then you can run it in the Matlab Help window, and the results will be displayed. See below, with the code you attached.

% Simulation parameters

dt = 0.1; % Time step

num_steps = 1000; % Number of time steps

% True state initialization

true_state = [10; 5; deg2rad(45); 1]; % Initial true state [r, r_dot, theta (in radians), theta_dot]

true_states = zeros(4, num_steps);

measurements = zeros(2, num_steps);

% Measurement noise covariance

R = [10, 0; 0, 1];

% Generate true states and measurements

for k = 1:num_steps

% Store true state

true_states(:, k) = true_state;

% Generate measurement with noise

measurement_noise = sqrtm(R) * randn(2, 1);

measurements(:, k) = h(true_state) + measurement_noise;

% Update true state using state transition function

true_state = f(true_state, dt);

end

% Initial state vector: [r, r_dot, theta, theta_dot]

x = [25; 2; deg2rad(25); 1];

P = eye(4) * 0.1;

% Process noise covariance

Q = eye(4) * 0.1;

% Extended Kalman Filter implementation

estimated_states = zeros(4, num_steps);

% Setup figure for real-time visualization

% figure;

% hold on;

legend_entries = [];

% Run EKF with real-time plotting

for k = 1:num_steps

% Prediction step

x_pred = f(x, dt);

F = F_jacobian(x, dt); % Use the updated Jacobian function

P_pred = F * P * F' + Q;

% Measurement update step

H = H_jacobian(x_pred);

y = measurements(:, k) - h(x_pred); % Measurement residual

% Correct theta angle wrap-around for residual

y(2) = atan2(sin(y(2)), cos(y(2)));

S = H * P_pred * H' + R; % Residual covariance

K = P_pred * H' / S; % Kalman gain

x = x_pred + K * y;

P = (eye(length(x)) - K * H) * P_pred;

% Store estimated state

estimated_states(:, k) = x;

% Plot the true and estimated values in real time

% plot(true_states(1, k), true_states(3, k), 'm+', 'MarkerSize', 5); % True position (r, theta)

% plot(estimated_states(1, k), estimated_states(3, k), 'rx', 'MarkerSize', 5); % Estimated position (r, theta)

% grid on;

% Update plot title and labels

% title('Real-Time EKF Estimation');

% xlabel('r (Radius)');

% ylabel('theta (Angle in radians)');

% Pause to simulate real-time visualization

% pause(0.01);

end

% Plot true and estimated values

t=dt*(0:num_steps-1);

figure

subplot(211)

plot(t,true_states(1,:),'g+',t,estimated_states(1, k),'r+'); % true & estim r

grid on; title('EKF Estimation')

ylabel('Radius'); legend('True','Estim')

subplot(212)

plot(t,true_states(3,:),'go',t,estimated_states(3, k),'ro'); % true & estim theta

grid on; xlabel('Time')

ylabel('Angle'); legend('True','Estim')

% State transition function (non-linear, converted to Cartesian coordinates)

function x_new = f(x, dt)

r = x(1);

r_dot = x(2);

theta = x(3);

theta_dot = x(4);

% Convert to Cartesian coordinates

x_cart = r * cos(theta);

y_cart = r * sin(theta);

x_dot_cart = r_dot * cos(theta) - r * theta_dot * sin(theta);

y_dot_cart = r_dot * sin(theta) + r * theta_dot * cos(theta);

% Update Cartesian coordinates

x_cart_new = x_cart + x_dot_cart * dt;

y_cart_new = y_cart + y_dot_cart * dt;

% Convert back to polar coordinates

r_new = sqrt(x_cart_new^2 + y_cart_new^2);

theta_new = atan2(y_cart_new, x_cart_new); % Angle in radians, no need to convert back

% Update radial and angular velocities

r_dot_new = (x_cart * x_dot_cart + y_cart * y_dot_cart) / r_new;

theta_dot_new = (x_cart * y_dot_cart - y_cart * x_dot_cart) / (r_new^2);

x_new = [r_new; r_dot_new; theta_new; theta_dot];

end

% Jacobian of the state transition function

function F = F_jacobian(x, dt)

r = x(1);

theta = x(3);

% Nonlinear Jacobian matrix approximation based on polar to Cartesian conversion

F = [1, dt, 0, 0;

0, 1, 0, 0;

0, 0, 1, dt;

0, 0, 0, 1]; % For simplicity, as higher order terms are neglected in this approximation

end

% Measurement function (non-linear)

function h_val = h(x)

r = x(1);

theta = x(3);

h_val = [r; theta]; % Measurement: [r; theta]

end

% Jacobian of the measurement function

function H = H_jacobian(x)

H = [1, 0, 0, 0; % Derivative of r with respect to [r, r_dot, theta, theta_dot]

0, 0, 1, 0]; % Derivative of theta with respect to [r, r_dot, theta, theta_dot]

end

I changed the code for plotting: I commented out the real-time plot updates. I added a figure showing true and estimated r and theta versus time, when the simulation and Kalman filtering is complete.

The plots shows that the extended Kalman filter, as implemented above, fails to estimate radius or theta successfully. I am not sure why. I am not experienced in using an extended Kalman filter.

The upper plot shows tha the true radius increases faster than linear. (Perhaps it increases parabolically or exponentially.) The true radius should increase linearly. Find pout why the radius is not behaving as expected.

The bottom plot shows that the phase increases linearly and at the expected rate (1 rad/s). The phase is discontinuous, due to wrapping from +pi to -pi as it increases. I suspect large and sudden changes in phase, which occur when the phase wraps around, will cause difficuly for the Kalman filter.

I hope to discuss this further later.

### Accepted Answer

William Rose
on 17 Oct 2024 at 5:41

Edited: William Rose
on 17 Oct 2024 at 5:45

[Edit: Fix spelling.] I modified your script to make it simpler. See attached file. To be specific, I removed the conversions from polar to Cartesian and back to polar. Those conversions occur inside function f(), the state transition function. With this change, the state transition function is a constant matrix, F. I used the same initial conditions and the same noise and covariance matrices that are in your script. Therefore my script implements a regular Kalman filter for a linear system, while yours implements an extended Kalman filter for a nonlinear system. My code works as expected. See figure below. The top panels are radius, the bottom panels are angle. The angle is not wrapped to (-pi,+pi). The left panels are the full 100 second simulation; the right panels are the last 5 seconds.

The figure above, especially the right hand panels, shows that the estimated state vector is less noisy than the measured state vector, and the estimated state vector tracks the true state vector very well. This indicates that the Kalman filter is working correctly.

I hope you can carefully add nonlinear aspects to my Kalman filter and keep it working as you do. I recommend that you keep the angle unwrapped, because I think the Kalman filter will have difficulty with abrupt shifts by 2*pi.

As I mentioned in an earlier comment, the "true" radius in your script does not increase linearly, as it should. The true radius in my script behaves as expected. I think the difference is due to something that is happening inside function f() in your script. I recommend you figure out that issue. Maybe fixing that issue will also help fix the extended Kalman filter in your script.

##### 0 Comments

### More Answers (0)

### See Also

### Categories

### Community Treasure Hunt

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

Start Hunting!