Kalman filter for FPGA in HDL Coder?
14 Ansichten (letzte 30 Tage)
Ältere Kommentare anzeigen
Jingyi Li
am 17 Jul. 2023
Beantwortet: Kiran Kintali
am 17 Jul. 2023
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 Kommentar
Akzeptierte Antwort
Kiran Kintali
am 17 Jul. 2023
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')
![](https://www.mathworks.com/matlabcentral/answers/uploaded_files/1436018/image.png)
0 Kommentare
Weitere Antworten (0)
Siehe auch
Community Treasure Hunt
Find the treasures in MATLAB Central and discover how the community can help you!
Start Hunting!