清空筛选条件
清空筛选条件

Constant estimation from 2 noisy measurements

1 次查看（过去 30 天）
Viktor Cockx
Viktor Cockx 2024-3-12
编辑： Aquatris 2024-4-12
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 个评论
Aquatris
Aquatris 2024-3-12
编辑：Aquatris 2024-3-12
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
Viktor Cockx 2024-3-13
How would you implement this ? I'm not entirely sure on how to do this. Thanks for the answer!

请先登录，再进行评论。

请先登录，再回答此问题。

回答（1 个）

Aquatris
Aquatris 2024-4-12
编辑：Aquatris 2024-4-12
Ran in:
@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

类别

在 Help Center 和 File Exchange 中查找有关 Online Estimation 的更多信息

标签

产品


版本

R2023b

Community Treasure Hunt

Find the treasures in MATLAB Central and discover how the community can help you!

Start Hunting!

Translated by