# create a extended kalman filter without extendedkalmanfilter in matlab

One way to create an extended Kalman filter without using Matlab's built-in `extendedKalmanFilter` function is by implementing the prediction and correction steps manually.

Firstly, define your system model and measurement model, which can be represented by state transition function `f` and observation function `h`. The state transition and observation model are:

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

where `x_k` is the state at time step `k`, `u_k` is the control input, `z_k` is the measurement at time step `k`, `w_k` and `v_k` are process and measurement noise respectively.

Next, initialize the initial state estimate `x_0`, the initial state covariance matrix P_0, and the process noise and measurement noise covariance matrices `Q` and `R`.

Then, for each time step, perform the following steps:

1. Prediction step: Use the system model to predict the state estimate at time `k`:
```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 chars3 lines```

where `F_k-1` is the Jacobian matrix of `f` at `x_k-1` and `u_k-1`.

1. Correction step: Use the measurement model to correct the state estimate at time `k`:
```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 chars4 lines```

where `H_k` is the Jacobian matrix of `h` at `x_k_pred`.

Repeat these steps for each time step.

Note that matrix multiplication is used extensively in both prediction and correction steps.

Here's an example implementation in Matlab:

```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 chars30 lines```

### related categories

gistlibby LogSnag