필터 지우기
필터 지우기

Kalman filter for FPGA in HDL Coder?

조회 수: 25 (최근 30일)
Jingyi Li
Jingyi Li 2023년 7월 17일
답변: Kiran Kintali 2023년 7월 17일
Hello, I am currently trying to make the kalman filter verilog code using HDL coder on MATLAB, to convert the MATLAB code into the Verilog/VHDL code.
To do this, I have to re-formulate my code into the format that the HDL coder accept.
My original code goes:
```
mass = 10^-12; % [kg]
dt = 10^-5; % Step time [s]
Nt = 10 / dt; % Number of time steps
Ts = (0:dt:Nt*dt); % Time plot
Omegas = 2*pi* [100 10 10]; % Spring frequnency; Testing 3 DOFs at maximum
Gammas = 2*pi* [0.1 0.1 1]; % Damping rate; Testing 3 DOFs at maximum
DOFs = 3; % Degrees of freedom; 1, 2, 3
Order = 3; % Orders for Exponential
% System matrix
A = zeros(2*DOFs,2*DOFs);
for dof = 1:DOFs
A(2*dof-1, 2*dof) = 1;
A(2*dof, 2*dof-1) = -Omegas(dof)^2;
A(2*dof, 2*dof) = -Gammas(dof);
end
% External force
kb = 1.38 * 10^(-23); % Boltzmann constant
T = 298;
% Var_sys = sqrt(2*mass*kb*T*Gammas); % System state noise due to thermal flucuations
Var_sys = sqrt(2*mass*kb*T*Gammas*10^10); % System state noise due to thermal flucuations
Var_measure = 0.0081*ones(1,3); % Measurement noise variance
fext_t = zeros(2*DOFs, length(Ts)); % external force divided by mass
% %{
% Ex.1. Parametric resonance
for dof = 1:DOFs
fext_t(2*dof,:) = 1*cos(Omegas(dof)*Ts);
end
rhs = @(t,x) A*x + 1*[0; cos(Omegas(1)*t); 0; cos(Omegas(2)*t); 0; cos(Omegas(3)*t)]; % rhs of function
xinit = repmat([1;0],DOFs,1); % initial value
[~,trueTrajectory] = ode45(rhs,Ts,xinit);
%}
% Display simulation conditions
fprintf('Simulation time = %.1f seconds, Bandwidth = %.1f kHz\n', Nt*dt, 1/dt/1000);
fprintf('Spectral radius of dt * A = %.5f.\n\n', abs(det(A))^(1/(2*DOFs))*dt);
for dof = 1:DOFs
fprintf(['%.0fst dimension: Spring frequency = %.1f Hz, damping rate = ' ...
'%.1f Hz \n'], dof, Omegas(dof)/2/pi, Gammas(dof)/2/pi);
end
rng;
noise_measure = sqrt(Var_measure).*randn(length(Ts),DOFs);
Trajectory_measureNoise = trueTrajectory(:,1:2:end) + noise_measure; %Add noise
% 2. Add system noise too
Trajectory_totalNoise = zeros(2*DOFs, length(Ts));
F = eye(2*DOFs);
for n = 2:Order
F = F+dt^(n-1)/factorial(n-1) * A^(n-1); % System propagation function
end
B = inv(A) * (F-eye(2*DOFs));
% Solve the time-dependent equation - Without force
Trajectory_totalNoise(:) = 0;
Trajectory_totalNoise(:,1) = xinit;
for tt = 2:size(Trajectory_totalNoise,2)
Trajectory_totalNoise(:,tt) = F*Trajectory_totalNoise(:,tt-1) + B * fext_t(:,tt);
Trajectory_totalNoise(1:2:end,tt) = Trajectory_totalNoise(1:2:end,tt) + sqrt(Var_sys)'.*randn(DOFs,1);
end
Trajectory_totalNoise(1:2:end,:) = Trajectory_totalNoise(1:2:end,:) +noise_measure';
% Trajectory_totalNoise = Trajectory_totalNoise(1:2:end,:);
figure,
subplot(311),
plot(Ts, Trajectory_totalNoise(1,:), 'color', [1 0 0 1], 'linewidth', 2), hold on
plot(Ts, Trajectory_measureNoise(:,1), 'color',[0.2 0.2 0.2 0.3], 'linewidth', 2), hold on
xlabel('Time (s)','fontname','Arial Nova Cond','FontSize', 10),
ylabel('Amplitude','fontname','Arial Nova Cond','FontSize', 10)
legend('x')
set(gcf,'color','w')
subplot(312),
plot(Ts, Trajectory_totalNoise(3,:), 'color', [0 1 0 1], 'linewidth', 2), hold on
plot(Ts, Trajectory_measureNoise(:,2), 'color',[0.2 0.2 0.2 0.3], 'linewidth', 2), hold on
xlabel('Time (s)','fontname','Arial Nova Cond','FontSize', 10),
ylabel('Amplitude','fontname','Arial Nova Cond','FontSize', 10)
legend('x')
set(gcf,'color','w')
subplot(313),
plot(Ts, Trajectory_totalNoise(5,:), 'color', [0 0 1 1], 'linewidth', 2), hold on
plot(Ts, Trajectory_measureNoise(:,3), 'color',[0.2 0.2 0.2 0.3], 'linewidth', 2), hold on
xlabel('Time (s)','fontname','Arial Nova Cond','FontSize', 10),
ylabel('Amplitude','fontname','Arial Nova Cond','FontSize', 10)
legend('x')
set(gcf,'color','w')
%% Kalman filter
C = eye(2*DOFs);
Q = repmat(Var_sys,2,1);
Q = diag(Q(:));% Noise on system, e.g. thermal noise
R = repmat(Var_measure,2,1);
R = diag(R(:));% Noise on measurement
% Initialize state estimate & covariance estimate
Trajectory_filtered = Trajectory_totalNoise*0;
x_jj = xinit; % init value
Trajectory_filtered(:,1) = x_jj;
Cov_jj = Q;
% Kalman filtering
for tt = 2:size(Trajectory_totalNoise,2)
% 1. Prediction
x_Jj = F * x_jj + B * fext_t(:,tt);
Cov_Jj = F * Cov_jj * F' + Q;
% 2. Observation and update
y_J = Trajectory_totalNoise(:,tt);
K_J = (Cov_Jj * C')/(C*Cov_Jj*C'+R);
x_jj = x_Jj + K_J* (y_J - C*x_Jj);
Cov_jj = (eye(2*DOFs) - K_J*C)*Cov_Jj;
Trajectory_filtered(:,tt) = x_jj;
end
%% .... more code
I found this example on the MATLAB website :
  댓글 수: 1
ProblemSolver
ProblemSolver 2023년 7월 17일
@Jingyi Li -- So what is the problem? What are you looking for?

댓글을 달려면 로그인하십시오.

채택된 답변

Kiran Kintali
Kiran Kintali 2023년 7월 17일
You need to break the MATLAB code into design and testbench and use MATLAB to HDL code advisor. See the sample example below.
>> mlhdlc_demo_setup('kalman')

추가 답변 (0개)

카테고리

Help CenterFile Exchange에서 Code Generation에 대해 자세히 알아보기

태그

Community Treasure Hunt

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

Start Hunting!

Translated by