Constant estimation from 2 noisy measurements

I'm trying to make an estimator to determine the mass of a vehicle based on the input torque and resulting longitudinal acceleration.

This means I have 2 noisy measurements which are related through the equation below. I have posed this problem to fellow students and gottan the answer to estimate the mass using a kalman filter, I've made the script below to do this. However i'm not certain this is the best possible solution since I find very little about using a kalman filter to estimate a constant. I'm also not sure how to best tune Q and R. Does anyone have experience with this?

Q=10e-14;
R=10e-10;
x_pred(1) = 800;  % Initial mass estimate
P_pred(1) = R; 

for i = 1:length(a_mes_n)-1
            % Measurement vector
            v_mes_n(i)=sum(a_mes_n(1:i))*ts;        % this is the velocity derived from the accel measurements
            F_aero_mes_n(i)=1/2*rho*SCx*v_mes_n(i)^2;   % aerodynamic drag based on this velocity
            z(i)=T_mes_n(i)/r_wheel./a_mes_n(i)-1/r_wheel^2*2*J_wheel-2*J_wheel*(1+Lambda_r)-F_aero_mes_n(i)/a_mes_n(i)-F_res/a_mes_n(i);
            
            % Prediction
            P_appri=P_pred(i)+Q;
            K(i)=P_appri/(P_appri+R);
            P_pred(i+1) = (1-K(i))*P_appri;
            x_pred(i+1) =  x_pred(i)+K(i)*(z(i)-x_pred(i)) ;  % 
end

[Aquatris]

To use Kalman filter to estimate a constant, or model parameter, you just introduce the constant variable as a state to your ODE in a way that is not affected by process noise or time.

[Viktor Cockx]

How would you implement this ? I'm not entirely sure on how to do this. Thanks for the answer!

Answer 1

[Aquatris]

@Viktor Cockx, A bit late but here is an example for a mass spring damper system, where I am trying to estimate the mass, spring coeff and damper coeff: (you should be able to adapt the script to your equation)

clear all,clc

% time vector for the simulation
dt = 1e-3;
t = 0:dt:100;

% actual stiffness, mass and damping of the system
k = 15;
m = 25;
d = 13;

% define covariances for Kalman filters
% these have a lot of effect on the estimations so keep that in mind play
% with them and see what happens
Q = diag([0 0 1 1 1]);
R = 1e1;

% actual initial states (last three are the stiffness,mass and damping constants)
x0 = [1 2 k m d]';

% myODE function wrapper
% mass*accel + damping*velocity + position*stiffness = F
myOdeFcn = @(t,x) myODE(t,x);

% simulate the system
[t,y] = ode45(myOdeFcn,t,x0);

% add sensor noise to the output, which is position of the system
rng(25)
yN = y+(2*rand(size(y))-1)*1e-2.*[1 0 0 0 0];

% initial conditions for the kalman filter, e stands for extended :P
xe= [0 0 1 1 1]'; % assume x=xd=0 and k=m=d=1

% intialize P matrix for kalman
Pe(:,:,1) = eye(5);

% simulate kalman filter
for i = 2:length(t)   
   [xNext,Ptmp] = extendedKalmanPredict(dt,t(i-1),xe(:,i-1),Pe(:,:,i-1),Q);
   [xUpdated,Ptmp,ee(i)] = extendedKalmanUpdate(xNext,yN(i,1),Ptmp,R);
   xe(:,i) = xUpdated;
   Pe(:,:,i) = Ptmp;
end
xe = xe'; % transpose to make it row vector

% plots
f = figure(1);
s(1) = subplot(2,2,1);plot(t,yN(:,1),'r-',t,y(:,1),'b',t,xe(:,1),'m')
ylabel('Position'),xlabel('Time')
legend('Noisy Position','Actual Position','Extended Kalman Filter That Estimate m,k,d') 
s(2) = subplot(2,2,3);plot(t,ee )
ylabel('Kalman Filter Position Estimation Errors'),xlabel('Time')
legend('Extended Kalman Filter That Estimate m,k,d')
s(3) = subplot(1,2,2);plot(t,xe(:,3:end)./[k m d])
ylabel('Accuracy of Estimation (1 = 100%)'),xlabel('Time')
grid on
grid minor
legend('k','m','d')
linkaxes(s,'x');

function u = myInput(t)
% define input force to excite the system to be able to estimate constants
% this is an important signal and should be chosen carefully, I just use a
% simple square way cause it works for the current settings, but might not
% work for different m k d values
      u = square(2*pi*0.03*t)*15;
end
function dxdt = myODE(t,x)
A = [0    1;
   -x(3)/x(4) -x(5)/x(4)];
A = [A         ,zeros(2,3)
   zeros(3,2),zeros(3)];
B = [0;1/x(4);0;0;0];

dxdt = A*x+B*myInput(t);
end

function [xNext,P] = extendedKalmanPredict(dt,t,x,P,Q)
if abs(x(4))<1e-2
   x(4) = sign(x(4))*1e-2; % prevent mass = 0;
end
% nonlinear state transition function 
% (nonlinear due to m k d being a state as well)
f = [x(1)+x(2)*dt
     x(1)*(-x(3)/x(4))*dt+x(2)+x(2)*(-x(5)/x(4))*dt+myInput(t)*dt/x(4)
     x(3)
     x(4)
     x(5)];
% take jacobian of f
F =[1              ,dt                 ,0             ,0                                                              ,0
    -x(3)/x(4)*dt  ,1+(-x(5)/x(4)*dt)  ,-x(1)/x(4)*dt ,x(1)*x(3)/x(4)^2*dt+x(2)*x(5)/x(4)^2*dt-myInput(t)*dt/x(4)^2   ,-x(2)/x(4)*dt
    0              ,0                  ,1             ,0                                                              ,0
    0              ,0                  ,0             ,1                                                              ,0
    0              ,0                  ,0             ,0                                                              ,1];
xNext = f;
P = F*P*F'+Q;
end
function [xUpdated,P,e] = extendedKalmanUpdate(x,z,P,R)
h = x(1); % output matrix
H = [1 0 0 0 0]; % jacobian of output matrix
y = z-h;
S = H*P*H'+R;
K = P*H'*S^-1;

xUpdated = x+K*y;
P = (eye(size(K*H))-K*H)*P;
e = z-H*xUpdated;
end