Constant estimation from 2 noisy measurements
3 views (last 30 days)
Show older comments
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
2 Comments
Answers (1)
Aquatris
on 12 Apr 2024
Edited: Aquatris
on 12 Apr 2024
@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
0 Comments
See Also
Categories
Find more on Online Estimation in Help Center and File Exchange
Community Treasure Hunt
Find the treasures in MATLAB Central and discover how the community can help you!
Start Hunting!