Kalman filter 2d

"My system matrix consists of r, r_dot, theta, and theta_dot, and my measurement matrix consists of r and theta. I want to apply a Kalman filter to this. Now, I need to convert the system's motion to Cartesian coordinates, propagate it in Cartesian, and then apply the Kalman filter. Could you help me with the Kalman filter steps for this or provide a source code example or someone who can assist me?"

[William Rose]

@kalmanfilterlearner,

Do you want to use the Kalman filter functions in Matlab's Control System toolbox, or do you plan to make your own Kalman filter, without the toolbox? Either is possible. In any case, you need to understand how to apply the theoretical ideas of a Kalman filter to your specific situation.

You may like the first 21 pages of this PDF as an introduction. The "Filling Model" on pp.16-21 has nice analogies to your system, since it includes an unmeasured variable which corresponds to the time derivative of the measured variable - just like in your system. However, your system has two measured and two unmeasured variables, instead of one and one, and you have polar-Cartesian conversions to deal with.

You may like Example 9 on this site. Think about how your system is similar to and different from Example 9, and make adjustments.

Make some simulated data, with measurement noise, if you do not already have some.

Lastly, please show the code you have written so far. Please explain what errors you get, or why you are not satisfied with the output.

[kalmanfilterlearner]

@William Rose

I am converting Cartesian for system motion but anyway i dont know am i doing a any mistake. The results shows me to definetely i do.

% 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

% 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

[William Rose]

@kalmanfilterlearner,

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.

Answer 1

[William Rose]

@kalmanfilterlearner,

[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.