Constant estimation from 2 noisy measurements
1 次查看(过去 30 天)
显示 更早的评论
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 个评论
回答(1 个)
Aquatris
2024-4-12
编辑:Aquatris
2024-4-12
@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 个评论
另请参阅
类别
在 Help Center 和 File Exchange 中查找有关 Online Estimation 的更多信息
Community Treasure Hunt
Find the treasures in MATLAB Central and discover how the community can help you!
Start Hunting!