Extended kalman filter jacobian function

I am trying to code my extended Kalman filter on a system and I know the function of the jacobian of the nonlinear output function. I read the documentation and look at the MeasurementJacobianFcn example, but I would need to pass to the function some parameters, is this possible?

[Umar]

Hi Camilla,

Yes, define your Measurement Jacobian function with additional input arguments to accommodate the parameters you need. Here is a simple example to illustrate how you can pass parameters to the Measurement Jacobian function:

function H = MeasurementJacobian(x, params)

    % Your code to calculate the Measurement Jacobian using the parameters

    % params

end

Then, you can pass the required parameters along with the Measurement Jacobian function when calling the Extended Kalman Filter function,

params = [param1, param2, ...]; % Define your parameters

ekf = extendedKalmanFilter(@StateTransitionFcn, @MeasurementFcn, initialState, 'HasAdditiveMeasurementNoise',true, 'MeasurementJacobianFcn', @(x) MeasurementJacobian(x, params));

By incorporating the additional parameters in the Measurement Jacobian function and passing them when setting up the Extended Kalman Filter, you can customize the behavior of the filter based on your specific requirements. To illustrate an example, I implemented an extended Kalman filter in Matlab with a Jacobian function that requires parameters by defining the system dynamics, measurement function, and the Jacobian function. This implementation provides a basic framework for an extended Kalman filter with a nonlinear measurement function and its Jacobian. I will walk you through the process step by step.

Define System Dynamics

First, define the system dynamics. For this example, let's consider a simple linear system:

% Define the state transition function A = [1 1; 0 1]; % State transition matrix B = [0.5; 1]; % Control input matrix f = @(x, u) A*x + B*u; % State transition function

Define Measurement Function and Jacobian

Next, define the measurement function and its Jacobian. Let's assume a simple measurement model:

% Define the measurement function and its Jacobian

h = @(x) x(1); % Measurement function (assuming measurement is the first state variable)

H = @(x) [1 0]; % Measurement Jacobian

Extended Kalman Filter Implementation

Now, implement the extended Kalman filter using the defined functions:

% Initialize state and covariance

x = [0; 0]; % Initial state estimate

P = eye(2); % Initial covariance matrix

% Process and measurement noise covariance

Q = eye(2); % Process noise covariance

R = 1; % Measurement noise covariance

% Data

u = 1; % Control input

z = 2; % Measurement

% Preallocate arrays for storing results

state_estimates = zeros(2, 10);

covariance_matrices = zeros(2, 2, 10);

% Extended Kalman Filter with plotting

for i = 1:10

    % Prediction step

    x = f(x, u);

    F = A; % State transition Jacobian

    P = F*P*F' + Q;

    % Update step

    y = z - h(x);

    Hx = H(x); % Update measurement Jacobian using a different variable name

    S = Hx*P*Hx' + R;

    K = P*Hx'/S;

    x = x + K*y;

    P = (eye(2) - K*Hx)*P;

    % Store results for plotting

    state_estimates(:, i) = x;

    covariance_matrices(:, :, i) = P;

    % Print results

    disp(['Iteration: ', num2str(i)]);

    disp(['State Estimate: ', num2str(x')]);

    disp(['Covariance Matrix:']);

    disp(P);

end

% Plotting state estimates

figure;

subplot(2, 1, 1);

plot(1:10, state_estimates(1, :), 'b-o');

xlabel('Iteration');

ylabel('State Estimate 1');

title('State Estimate 1 over Iterations');

subplot(2, 1, 2);

plot(1:10, state_estimates(2, :), 'r-o');

xlabel('Iteration');

ylabel('State Estimate 2');

title('State Estimate 2 over Iterations');

% Plotting covariance matrices with updated titles

figure;

for i = 1:10

    subplot(2, 5, i);

    imagesc(covariance_matrices(:, :, i));

    colorbar;

    title(['Covar. Mat.- Iter. ', num2str(i)], 'Interpreter', 'none'); % Update title with iteration number

end

Results

Please see attached plots and results.

When you run the code, it will iterate through the extended Kalman filter steps and print the state estimate and covariance matrix at each iteration. You can adjust the system dynamics, measurement function, and noise covariances based on your specific system. Also, added arrays to store state estimates and covariance matrices for each iteration. I then included plotting functions to visualize the evolution of state estimates and covariance matrices over the iterations of the Extended Kalman Filter. The plots provide a visual representation of how the estimates and uncertainties change with each iteration, aiding in understanding the filter's performance. You can further enhance it by incorporating more complex system dynamics and measurement models as needed. Feel free to customize the code according to your specific requirements and system characteristics.