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.

[Camilla Ancona]

Thanks a lot for your thorough and detailed answer. I will try to use your code, but in the mea time i tried to use the command extendedkalmanfilter build in in matlab and i wanted to code my own measurement jacobian with some parameters to feed the object creates with extendedkalmanfilter command. Is it possible?

[Umar]

Hi Camilla,

Yes, it is possible. By customizing the measurement Jacobian, you can enhance the performance and accuracy of your extended Kalman filter for state estimation tasks. Remember to ensure that your custom Jacobian matrix is correctly computed and matches the dimensions expected by the extendedKalmanFilter function to integrate it seamlessly into your estimation process. I actually implemented an Extended Kalman Filter by defining the nonlinear state transition function f, the measurement function h, and then construct the EKF object with the necessary parameters.

% Define the nonlinear state transition function f

f = @(x, u) [x(1) + u(1)*cos(x(3)); x(2) + u(1)*sin(x(3)); x(3) + u(2)];

% Define the measurement function h

h = @(x) [sqrt(x(1)^2 + x(2)^2); atan2(x(2), x(1))];

% Construct the extendedKalmanFilter object

ekf = extendedKalmanFilter(f, h);

% Specify the noise terms as additive

ekf.ProcessNoise = eye(3)*0.01;

ekf.MeasurementNoise = eye(2)*0.1;

% Generate Jacobians of the state transition and measurement functions

ekf.StateTransitionJacobianFcn = @(x, u) [1, 0, -u(1)*sin(x(3)); 0, 1, u(1)*cos(x(3)); 0, 0, 1];

ekf.MeasurementJacobianFcn = @(x) [x(1)/sqrt(x(1)^2 + x(2)^2), x(2)/sqrt(x(1)^2 + x(2)^2), 0; -x(2)/(x(1)^2 + x(2)^2), x(1)/(x(1)^2 + x(2)^2), 0];

disp('Extended Kalman Filter object created successfully.');

So, in my provided code snippet, the nonlinear state transition function f and the measurement function h are defined using anonymous functions then an extendedKalmanFilter object ekf is created with the defined functions. I processed noise and measurement noise which are specified as additive with covariance matrices. State transition and measurement Jacobians are generated using anonymous functions. Finally, a message confirming the successful creation of the EKF object is displayed. Please see attached results.

[Camilla Ancona]

The following error shows when I try to use my custom function to evalueate the measurement jacobian

Error using matlabshared.tracking.internal.ExtendedKalmanFilter/validateMeasurementJacobianFcn (line 1990). The number of optional arguments in the call to residual does not match the number of additional arguments expected by the MeasurementJacobianFcn. Check that all additional arguments of MeasurementJacobianFcn are provided as optional input arguments the residual.

obj_p = extendedKalmanFilter(@my_sist_gradino_d,...

@my_volt_td_p,conc_in2(1:q,:),'ProcessNoise',0,'MeasurementNoise',varianza);

obj_p.MeasurementJacobianFcn = @pos_electrodeMeasurementJacobianFcn;

for k = 1:num_steps_short

[Residual,~] = residual(obj_p,Volt_td_short_p(k),...

q,d_N,d_N1, d_N2, beta_p, c_p_max);

[CorrectedState,~] = correct(obj_p,Volt_td_short_p(k),...

q,d_N,d_N1, d_N2, beta_p, c_p_max);

Theta1pos(ss,k) = d2*CorrectedState(1) + d3*CorrectedState(2);

ThetaNpos_d(ss,k) = d_N1*CorrectedState(q) + d_N2*CorrectedState(q-1) + (d_N*beta_p/c_p_max);

[PredictedState,~] = predict(obj_p, Ad_p,Bd_p, u_new(1));

residBuf_p(ww,ss,k) = Residual;

xcorBuf_p(ww,ss,k,:) = CorrectedState';

xcorBuf_tot_pos(ww,ss,k,:) = [Theta1pos(ss,k) CorrectedState' ThetaNpos_d(ss,k)];

xpredBuf_p(ww,ss,k,:) = PredictedState';

end

function C_P = pos_electrodeMeasurementJacobianFcn(Theta_td, rho)

d_N2 = -(rho(N)-rho(N-1))^2/((rho(N-1)-rho(N-2))*(2*rho(N)-rho(N-1)-rho(N-2)));

d_N1 = (rho(N)-rho(N-2))^2/((rho(N-1)-rho(N-2))*(2*rho(N)-rho(N-1)-rho(N-2)));

d_N = ((rho(N)-rho(N-1))*(rho(N)-rho(N-2)))/(2*rho(N)-rho(N-1)-rho(N-2));

theta_p = d_N1*Theta_td(:,q) + d_N2*Theta_td(:,q-1) + (d_N*beta_p/c_p_max);

%%theta_p = ThetaNpos(kk); questo ti serve per ricordarti cosa passargli

C_tildeP2 = d_N2*((6*85.681)*theta_p^5 -(5*373.7)*theta_p^4 + (4*613.89)*theta_p^3 -(3*555.65)*theta_p^2 +...

(2*281.06)*theta_p -76.648 -(115*5.657*theta_p^114)*(0.30987)*exp(5.657*theta_p^115));

C_tildeP1 = d_N1*((6*85.681)*theta_p^5 -(5*373.7)*theta_p^4 + (4*613.89)*theta_p^3 -(3*555.65)*theta_p^2 +...

(2*281.06)*theta_p -76.648 -(115*5.657*theta_p^114)*(0.30987)*exp(5.657*theta_p^115));

C_P = [zeros(1,N-4) C_tildeP2 C_tildeP1];

end

[Umar]

Hi Camilla,

To resolve this issue, ensure that all additional arguments required by the MeasurementJacobianFcn are passed as optional input arguments to the residual function. Check the implementation of the pos_electrodeMeasurementJacobianFcn function to verify the correct number of arguments and their types are being passed. Make sure the arguments match between the residual and MeasurementJacobianFcn functions to align the optional input arguments properly.

Answer 1

[Rahul]

Hi @Camilla Ancona,

I understand that you wish to pass some parameters to the Jacobian function of your extended Kalman Filter.

One of the ways you can achieve this is by defining your own Jacobian function.

function J = myJacobianFunction(state, param1, param2)
    
    J = [param1, 0; 0, param2];
end

% This is an example it can be changed as per requirements.

param1 = 1.0;
param2 = 2.0; 

jacobianFcnHandle = @(state) myJacobianFunction(state, param1, param2);

% Function handle for the Jacobian function

obj = extendedKalmanFilter(...
    @stateTransitionFcn, ...
    @measurementFcn, ...
    'MeasurementJacobianFcn', jacobianFcnHandle);

% Adding the function handle as the 'MeasurementJacobianFcn' of the extended Kalman filter

You can refer to the following documenatations for the detailed instructions on how to use these functions:

"extendedKalmanFilter": https://www.mathworks.com/help/control/ref/extendedkalmanfilter.html

"generateJacobianFcn": https://www.mathworks.com/help/control/ref/extendedkalmanfilter.generatejacobianfcn.html#d126e62582

Hope this helps! Thanks.