Kalman filter 2d

5 次查看（过去 30 天）
kalmanfilterlearner
kalmanfilterlearner 2024-10-14
编辑： William Rose 2024-10-17
"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?"
  3 个评论
显示 1更早的评论
kalmanfilterlearner
kalmanfilterlearner 2024-10-15
@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
William Rose 2024-10-16
Ran in:
@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.

请先登录，再进行评论。

请先登录，再回答此问题。

采纳的回答

William Rose
William Rose 2024-10-17
编辑：William Rose 2024-10-17
@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.

更多回答（0 个）

类别

在 Help Center 和 File Exchange 中查找有关 Tracking Filters and Motion Models 的更多信息

标签

Community Treasure Hunt

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

Start Hunting!

Translated by