create a extended kalman filter without extendedkalmanfilter in matlab

main.m
x_k = f(x_{k-1}, u_{k-1}) + w_{k-1}
z_k = h(x_k) + v_k
55 chars
3 lines

main.m
x_k_pred = f(x_{k-1}, u_{k-1})
P_k_pred = F_{k-1} * P_{k-1} * F_{k-1}^T + Q_{k-1}
82 chars
3 lines

main.m
K_k = P_k_pred * H_k^T * (H_k * P_k_pred * H_k^T + R_k)^-1
x_k_corr = x_k_pred + K_k * (z_k - h(x_k_pred))
P_k_corr = (I - K_k * H_k) * P_k_pred
145 chars
4 lines

main.m
% Define system and measurement models
f = @(x, u) [x(1) + u(1)*cos(x(3))*delta_t; 
             x(2) + u(1)*sin(x(3))*delta_t;
             x(3) + u(2)*delta_t];
h = @(x) [sqrt(x(1)^2 + x(2)^2);
          atan2(x(2), x(1))];
          
% Initialize initial state estimate, covariance matrix, and noise covariance
x_estimate = [0 0 0]';
P = eye(3);
Q = diag([0.1 0.1 0.01]);
R = diag([0.1 0.1]);

% Loop through each time step
for k = 1:N
    % Prediction step
    F_k = [1 0 -u(1)*sin(x_estimate(3))*delta_t;
           0 1  u(1)*cos(x_estimate(3))*delta_t;
           0 0        1];
    x_estimate_pred = f(x_estimate, u);
    P_pred = F_k * P * F_k' + Q;
    
    % Correction step
    H_k = [x_estimate_pred(1)/sqrt(x_estimate_pred(1)^2 + x_estimate_pred(2)^2) x_estimate_pred(2)/sqrt(x_estimate_pred(1)^2 + x_estimate_pred(2)^2) 0;
           -x_estimate_pred(2)/(x_estimate_pred(1)^2 + x_estimate_pred(2)^2) x_estimate_pred(1)/(x_estimate_pred(1)^2 + x_estimate_pred(2)^2) 0];
    K = P_pred * H_k' * inv(H_k * P_pred * H_k' + R);
    x_estimate = x_estimate_pred + K * ([ranges(k); bearings(k)]' - h(x_estimate_pred));
    P = (eye(3) - K * H_k) * P_pred;
end
1167 chars
30 lines