Jag har tittat mycket på lågpassfiltrering. Ett klassiskt filter som man kan antingen ha digitalt eller analogt. Sedan finns det lite mer avancerade filter så som FIR-filter och Butterworth filter. Dom båda kan vara digitala men endast Butterworth kan implementeras som både analogt eller digitalt.
Nackdelen med filtrering är att man kan få fasförskjutning på sin filtrerade signal, dvs fördröjning. För att komma runt detta så kan man filtrera med "halva styrkan" och sedan vänta på signalen och filtrera med "halva styrkan" igen och sedan vända på signalen. Detta kallas filtfilt inom moderna mjukvaror. Resultatet blir att fasförskjutningen förekommer inte.
Nackdelen med att göra filtfilt är att man måste ha en hög samplade värden först innan man kan börja filtrera.
Enklaste filtreringen och enligt mig är glidande medelvärde. Men glidande medelvärde ser bara bra ut om man inte känner till andra typer av filtrering. Dessutom skapar glidande medelvärde riktiga fasförskjutningar.
Så oavsett vad för filtrering man använder, så blir det alltid fasförskjutning då ett filter innehåller dynamik för att "sega" ned variationerna. Utom....om man använder estimering. Estimering är alltså beräknad uppskattning och då använder man statistik istället för dynamik. Att estimera ett framtida värde tog kraft på 60-talet när Rudolf E. Kalman uppfann det så kallade linjära kalman filtret. Det var ett filter som var logiskt, enkelt och robust. Varför ingen hade tänkt på detta innan kan man fråga. Filtret var så enkelt att det implementerades till och med i datorer på Apolloprogrammet. På den tiden kallades det för Linear Quadratic Gaussian och detta är en linjärkvadratisk regulator (Linear Quadratic Regulator) (optimering) som innehåller ett gaussiskt filter.
Gaussiskt, uppkallat efter linjära algebrans fader Carl Friedrich Gauss, är det som är naturligt normalt fördelat. En gaussiskt fördelad spridning ser bland annat ut så här i praktiken. Detta är från en 35 kr lasergivare som mäter på ett fixat avstånd på 14mm. Ni ser hur det sprider sig ungefär lika mycket på båda sidorna. Målet med filtrering och estimering är att hitta medelvärdet, i detta fall högsta toppen på denna kurva. Problemet verklig mätning är att medelvärde och spridningen varierar över tid. Det är därför man alltid får variationer över tid trots att man har implementerat sitt perfekta filter.
Då passar estimering riktigt bra här. Estimeringen är alltså ett filter som beräknar nästa förutsebara mätvärde igenom att titta på gammal data. Det fina med estimering är att man behöver bara minst 3 punkter att mäta mellan för att få ett bra mätvärde.
Jag har bland annat testat Kalman Filter(KF), Extended Kalman Filter(EKF) och Unscented Kalman Filter(UKF) och UKF är den bästa av alla då KF är för linjärt brus, dvs perfekt gaussiskt fördelat samt EKF är bara jobbigt matematiskt då man måste hålla på med jacobianer(linjärisering). UKF är dessutom enklast av alla i praktiken, trots att den innehåller flest matematiska symboler.
Skumma gärna igenom matematiken här: https://en.wikipedia.org/wiki/Kalman_fi ... man_filter
UKF bygger på att man använder så kallade "sigma points" för att bestämma den gaussiska kurvans form och position. Alla kalman filter så måste man använda en transitions-funktion, och i detta fall har jag valt en slumpvariabel som transitions-funktion.
Sedan kollar ni på resultatet mellan glidande medelvärde, UKF och verklig mätning här. Frågeställning:
Är det värt att implementera ett UKF filter på inbyggda system?
Jag planerar att skriva om denna MATLAB kod till C-kod. Testa kör den.
MATLAB/GNU Octave kod finns nedan och log.txt finns här
Kod: Markera allt
function Unscented_Kalman_Filter()
% Initial parameters
L = 1;
a = 0.001;
b = 2;
k = 1;
Q = 0.0001;
R = 2;
P = 1;
xhat = 15;
zk = 13;
% Read the data
fid = csvread('log.txt')';
l = 500;
% Filtered data
X = zeros(1, l);
% Sliding average
count = 1;
samples_avg = 20;
Y = [];
avg = 0;
for i = 1:l
% Get measurement
zk = fid(i);
% Save UKF value
X(i) = xhat;
% Sliding average
avg = avg + zk;
if(count >= samples_avg)
Y = [Y avg/count];
avg = 0;
count = 1;
else
count = count + 1;
end
% UKF filter
[xhat, P] = ukf(xhat, zk, P, Q, R, a, k, b, L);
end
close all
plot(1:l, X, 'r', 1:l, fid(1:l), 'b', 1:length(Y), Y, 'g');
legend('UKF', 'Real', 'Sliding')
grid on
end
function [xhat, P] = ukf(xhat, zk, P, Q, R, a, k, b, L)
% Init - Create the weights
[Wa, Wc] = ukf_create_weights(a, b, k, L);
% PREDICT: Step 0 - Predict the state and state estimation error covariance at the next time step
s = ukf_compute_sigma_points(xhat, P, a, k, L);
% PREDICT: Step 1 - Run our transition function
x = ukf_transition(s, L);
% PREDICT: Step 2 - Combine the predicted states to obtain the predicted states at time
xhat = ukf_multiply_weights(x, Wa, L);
% PREDICT: Step 3 - Compute the covariance of the predicted state
P = ukf_estimate_covariance(x, xhat, Wc, Q, L);
% UPDATE: Step 1 - Use the nonlinear measurement function to compute the predicted measurements for each of the sigma points.
z = s; % Here we assume that the observation function z = h(s, u) = s
% UPDATE: Step 2 - Combine the predicted measurements to obtain the predicted measurement
zhat = ukf_multiply_weights(z, Wa, L);
% UPDATE: Step 3 - Estimate the covariance of the predicted measurement
Shat = ukf_estimate_covariance(z, zhat, Wc, R, L);
% UPDATE: Step 4 - Estimate the cross-covariance between xhat and yhat. Here i begins at 1 because xhati(0) - xhat(0) = 0
Csz = ukf_estimate_cross_covariance(s, xhat, z, zhat, Wc, L);
% UPDATE: Step 5 - Find kalman K matrix
K = ukf_create_kalman_K(Shat, Csz, L);
% UPDATE: Step 6 - Obtain the estimated state and state estimation error covariance at time step
[xhat, P] = ukf_state_update(K, Shat, P, xhat, zk, zhat, L);
end
function [Wa, Wc] = ukf_create_weights(a, b, k, L)
N = 2 * L + 1;
Wa = zeros(1, N);
Wc = zeros(1, N);
for i = 1:N
if(i == 1)
Wa(i) = (a*a*k-L)/(a*a*k);
Wc(i) = Wa(i) + 1 - a*a + b;
else
Wa(i) = 1/(2*a*a*k);
Wc(i) = Wa(i);
end
end
end
function [s] = ukf_compute_sigma_points(x, P, a, k, L)
N = 2 * L + 1;
compensate = L + 1;
s = zeros(L, N);
A = a*sqrt(k)*chol(P);
for j = 1:N
if(j == 1)
s(:, j) = x;
elseif(and(j >= 2, j <= L + 1))
s(:, j) = x + A(:, j - 1);
else
s(:, j) = x - A(:, j - compensate);
end
end
end
function x = ukf_multiply_weights(xi, W, L)
N = 2 * L + 1;
x = zeros(L, 1);
for i = 1:N
x = x + W(i)*xi(:, i);
end
end
function P = ukf_estimate_covariance(xi, x, W, O, L)
N = 2 * L + 1;
P = zeros(L, L);
for i = 1:N
P = P + W(i)*(xi(:, i) - x)*(xi(:, i) - x)';
end
P = P + O;
end
function P = ukf_estimate_cross_covariance(xi, x, yi, y, W, L)
N = 2 * L + 1;
P = zeros(L, L);
for i = 1:N
P = P + W(i)*(xi(:, i) - x)*(yi(:, i) - y)';
end
end
function K = ukf_create_kalman_K(Shat, Csz, L)
K = zeros(L, L);
for i = 1:L
% Solve Ax = b with Cholesky
A = chol(Shat, 'lower');
y = linsolve(A, Csz(:, i));
K(:, i) = linsolve(A', y);
end
end
function [xhat, P] = ukf_state_update(K, Shat, P, xhat, zk, zhat, L)
xhat = xhat + K*(zk - zhat);
P = P - K*Shat*K';
end
function x = ukf_transition(s, L)
N = 2 * L + 1;
x = zeros(L, 1);
for i = 1:N
x(i) = std(s(:, i))*randn + mean(s(:, i)); % std*random_variable + average = Gaussian distribution
end
end
