# Kalman filter for FPGA in HDL Coder?

조회 수: 25 (최근 30일)
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이전 댓글 -1개 표시이전 댓글 -1개 숨기기
ProblemSolver 2023년 7월 17일
@Jingyi Li -- So what is the problem? What are you looking for?

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

### 채택된 답변

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')

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

### 카테고리

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