Guidance regarding implementation of UKF in Matlab

I am trying to implement a UKF for a mechatronic aeropendulum system and I have tried to follow the example of the UKF using the Van Der Pol oscillator example from here https://in.mathworks.com/help/control/ug/nonlinear-state-estimation-using-unscented-kalman-filter.html

my functions are as follows:

%--------------------------------------------------
% Aeropendulum Dynamic Equation
% (mL^2) * θ'' + c θ' + mglsin(θ) = Ku
% x(1) = θ 
% x(2) = θ'
%--------------------------------------------------


function dxdt = AeropendulumDynamics(x,m,c,g,l,K,u)
    dxdt = [x(2);(-c*x(2) - m*g*l*sin(x(1)) + K*u) / (m*l^2)];
end


function x_next = AeroPendStateXsnFunc(x,m,c,g,l,K,u,dt)
    % One-step forward Euler integration
    x_next = x + AeropendulumDynamics(x,m,c,g,l,K,u) * dt;
end


function yk = AeroPendMeasurementNonAdditiveNoiseFunc(xk,vk)
yk = xk(1)*(1+vk);
end

And the main function is as follows :

clc;clear;close;

%aeropendulum parameters
m = 2;
c = 3;
l = 0.8;
g = 9.8;
K = 10;
u = 1;

%simulation parameters
t_step = 0.01;
t_end = 5;
t_span = 0:t_step:t_end;
x_initial = [0;0];



stateFcn = @(x) AeroPendStateXsnFunc(x, m, c, g, l, K, u, t_step);
measFcn  = @(x,v) AeroPendMeasurementNonAdditiveNoiseFunc(x, v);
x_initial_guess = [1;0];
%construct ukf
ukf = unscentedKalmanFilter(...
    stateFcn, ...
    measFcn, ...
    x_initial_guess, ...
    'HasAdditiveMeasurementNoise', false);


R = 0.01; % Variance of the measurement noise v[k]
ukf.MeasurementNoise = R;
ukf.ProcessNoise = diag([0.2 0.3]);

[tTrue, xTrue] = ode45(@(t,x)AeropendulumDynamics(x,m,c,g,l,K,u), t_span, x_initial);


rng(1); % Fix the random number generator for reproducible results
yTrue = xTrue(:,1);
yMeas = yTrue .* (1+sqrt(R)*randn(size(yTrue))); % sqrt(R): Standard deviation of noise

Nsteps = numel(yMeas); % Number of time steps
xCorrectedUKF = zeros(Nsteps,2); % Corrected state estimates
PCorrected = zeros(Nsteps,2,2); % Corrected state estimation error covariances
e = zeros(Nsteps,1); % Residuals (or innovations)

for k=1:Nsteps
    e(k) = yMeas(k) - AeroPendMeasurementNonAdditiveNoiseFunc(ukf.State, 0);
    [xCorrectedUKF(k,:), PCorrected(k,:,:)] = correct(ukf,yMeas(k));
    predict(ukf);
end

hold on
plot(tTrue,rad2deg(xTrue(:,1)))
plot(tTrue,rad2deg(yMeas))
plot(tTrue,rad2deg(xCorrectedUKF(:,1)))
grid

I do not understand why my filter completely trust my sensor yMeas I think I have gone wrong somewhere...

I also want to know how and where I can add process noise here as the example ignores process noise.

Answer 1

[Soumya]

Hi @Aditya,

I understand that you are implementing an Unscented Kalman Filter (UKF) for a mechatronic aeropendulum system and following the Van der Pol oscillator example from MathWorks. From your description and code, it seems the UKF is overly trusting the sensor measurements (yMeas), and you're also unsure how to properly incorporate process noise into your simulation. These are common challenges when adapting UKF to nonlinear systems, especially with non-additive noise models.

The core issue appears to be a mismatch between how measurement noise is modeled and how it is simulated. Your measurement function uses a multiplicative noise model '(yk = xk(1)*(1+vk))', but the UKF's 'MeasurementNoise' parameter is set as a fixed scalar (R = 0.01). Since the noise scales with the signal magnitude, this fixed value underestimates the actual uncertainty, causing the filter to overtrust the measurements. Additionally, the order of operations in your UKF loop calling correct before predict can lead to suboptimal state updates.

To resolve this, you can follow the given steps:

• Decide whether you want to use additive or multiplicative measurement noise.
• For simplicity and better compatibility with the UKF, switch to additive noise:
• Change your measurement function to:

yk = xk(1) + vk;

• Set:

ukf.HasAdditiveMeasurementNoise = true;

• Simulate measurements using:

yMeas = yTrue + sqrt(R) * randn(size(yTrue));

• Tune the Measurement Noise Covariance: If you prefer to keep the multiplicative model, increase R to reflect the actual variance. Try values like R = 0.05 or R = 0.1 and observe the filter's behavior.
• Correct the UKF Update Order: Always call predict before correct in your loop:matlabpredict(ukf):

[xCorrectedUKF(k,:), PCorrected(k,:,:)] = correct(ukf, yMeas(k));

• Add Process Noise to the True System: Your UKF models process noise internally, but your simulation (xTrue) does not reflect it. Modify your integration loop to include noise:

processNoiseStd = sqrt(diag(processNoiseCov));

for k = 2:length(t_span)
    
    dx = AeropendulumDynamics(xTrue(k-1,:)', m, c, g, l, K, u);
    
    noise = processNoiseStd .* randn(2,1);
    
    xTrue(k,:) = xTrue(k-1,:) + (dx + noise)' * t_step;
    
end

These changes should help your UKF balance trust between the model and the measurements more effectively.

You can refer to the following documentation on UKF and nonlinear state estimation:

• https://www.mathworks.com/help/control/ref/ukf_block.html
• https://www.mathworks.com/help/control/ug/nonlinear-state-estimation-using-unscented-kalman-filter.html

I hope this helps!