# create extended kalman in matlab

To create an extended Kalman filter in MATLAB, follow these steps:

1. Define the state-space model of the system you wish to estimate. This will include equations describing the dynamics of the system as well as any measurements that you have available.
```main.m```% Define the state-space model
syms x y theta v omega N1 N2
f = [x + (v/omega)*(-sin(theta) + sin(theta + omega*N1));
y + (v/omega)*(cos(theta) - cos(theta + omega*N1));
theta + omega*N1 + omega*N2];
g = [x; y; theta];
h = [x; y; theta];
```248 chars8 lines```
1. Initialize the filter by defining the initial state of the system and the covariance matrix for the initial estimate.
```main.m```% Initialize the filter
x0 = [0; 0; 0]; % initial state estimate
P0 = diag([1, 1, pi/4]); % initial covariance matrix
```118 chars4 lines```
1. Specify the noise characteristics of the system and the sensors.
```main.m```% Specify noise characteristics
Q = diag([0.1, 0.1, 0.01]); % process noise
R = diag([0.1, 0.1, 0.01]); % measurement noise
```124 chars4 lines```
1. Implement the extended Kalman filter.
```main.m```% Implement the extended Kalman filter
x = x0;
P = P0;
for i = 1:length(data)
% Prediction step
F = jacobian(f, [x; v; omega]);
G = jacobian(g, [x; v; omega]);
x = simplify(subs(f, [x; v; omega], [x; data(i,1); data(i,2)]));
P = simplify(F*P*F.' + G*Q*G.');

% Update step
H = jacobian(h, [x; v; omega]);
y = simplify(subs(h, [x; v; omega], [x; data(i,3); data(i,4)])) - y_hat;
S = H*P*H.' + R;
K = P*H.'/S;
x = simplify(x + K*y);
P = simplify((eye(3) - K*H)*P);

% Store results
x_est(:,i) = x;
P_est(:,i) = diag(P);
end
```590 chars23 lines```

This code defines a state-space model for a system with position (x,y) and heading angle theta, moving at a constant speed v and turning at a constant rate omega. The measurements available are the position (x,y) and heading angle theta, but they are corrupted by noise. The extended Kalman filter algorithm is used to estimate the state of the system at each timestep based on the available measurements.

Note that this is just an example and the actual implementation may vary depending on the specific system being modeled and estimated.

### related categories

gistlibby LogSnag