design kalman filter for radar tracking witout using functions in matlab

main.m
% State vector: [x, y, vx, vy] where (x, y) is the position and (vx, vy) is the velocity
x = [initial_x; initial_y; initial_vx; initial_vy];
% Measurement vector: [range, angle] where range is the distance from the radar and angle is the angle of the target
z = [initial_range; initial_angle];
% State covariance matrix
P = eye(4); % assumes equal and small initial uncertainty for position and velocity
% Process noise covariance matrix
Q = diag([q_pos; q_pos; q_vel; q_vel]); % assumes equal and small process noise for position and velocity
% Measurement noise covariance matrix
R = diag([r_range; r_angle]); % assumes equal and small measurement noise for range and angle
676 chars
11 lines

main.m
dt = 1; % time step
A = [1 0 dt 0; 0 1 0 dt; 0 0 1 0; 0 0 0 1]; % assumes constant velocity model
98 chars
3 lines

main.m
x = A * x; % predict the next state
P = A * P * A' + Q; % predict the next state covariance
92 chars
3 lines

main.m
% Compute the measurement matrix H based on the state vector x
H = [sqrt(x(1)^2 + x(2)^2) 0 0 0; x(2) / sqrt(x(1)^2 + x(2)^2) 0 x(1) / sqrt(x(1)^2 + x(2)^2) 0];
% Compute the residual between the predicted measurement and the actual measurement
residual = z - H * x;
% Compute the Kalman gain
K = P * H' / (H * P * H' + R);
% Update the state vector and the covariance matrix
x = x + K * residual;
P = (eye(4) - K * H) * P;
424 chars
10 lines