Hur många av er har kommit i kontakt med avancerade mätmetoder? Sensorfusion

[rohol]

Nu är jag ingen expert på partikelfilter, som sagt bara kommit i kontakt med det under mina studier. Men jag skulle säga att dina pseudopunkter 1-4 är identiska för ett partikelfilter.
Partikelfiltret kan också beskrivas med en mätuppdatering och tidsuppdatering, precis som Kalman filtret.

Ett PF kräver inte mer mätdata än ett KF.

Jag tror däremot att jag förstår vad det är som får dig till att tro annat så jag ska göra ett försök att förklara.

KF och PF kräver en modell, ett KF antar gaussiskt processbrus och man måste ge kovariansen för detta, ofta Q som sedermera ofta är en av designvariablerna.
För ett PF måste du dels ha systemmodellen men du måste också beskriva täthetsfunktionen för processbruset och du måste kunna dra samples från denna.
KDE (Kernel Density Estimation) är en metod för att estimera täthetsfunktionen, sen finns det metoder för att dra samples från den estimerade täthetsfunktionen eller att man kanske identifierar en känd täthetsfunktion som ser ut att passa in på beskrivningen av processbruset. När filtret sen kör så används inte KDE, KDE hör till förarbetet av implementationen.
Så i modelleringsarbetet kan det vara så att du får lägga ner mer energi för ett PF än ett KF, däremot kan du ju applicera PF på samma modell som i KF dvs gaussiskt processbrus, medelvärde 0 och kovarians Q. Då bör förarbetet vara densamma.

Nu har ju partikelfiltret en hel del partiklar men man tar helt enkelt det skattade tillståndet, drar en massa slumpmässiga partiklar med hjälp av den modellen man har.
Man gör en prediktionen av dessa partiklar (som kan vara säg 1000 st) och när ett mätvärde kommer då skattar man sannolikheten varje partikel har för det mätvärdet och detta blir vikten. Sedan väljer man den partikeln med högst sannolikhet som sitt skattade tillstånd.
Sen predikterar man med sina partiklar än en gång och ett nytt mätvärde kommer in och man skattar på nytt sannolikheten för varje partikel osv. Däremot så har partiklarna en tendens till att dö ut, dvs de blir väldigt osannolika, därför brukar man resampla beroende på något kriterie, t ex en threshold på vikterna i kombination med antalet partiklar. När man anser att för många partiklar dött ut då slänger man de partiklarna man har och tar sin modell och genererar nya partiklar precis som i början men nu utgår allt från det nuvarande skattade tillståndet.

PF är mer beräkningstungt och det blir ju en hel del funktionsevalueringar av modellen då man måste hålla koll på alla partiklarna men partiklarna kommer inte från mätvärden utan de är genererade m.h.a modellen och man evaluerar allteftersom mätvärden kommer in hur sannolika varje partikel är och i varje tidssteg väljer den mest sannolika som sin tillståndsskattning (man skulle kunna se dom som parallella simuleringar av systemet).

[rvl]

Den beskrivningen är jag beredd att skriva under.

Så länge man har gaussiskt brus så är KF optimalt, så då finns det ingen anledning att krångla till det med PF.

[DanielM]

Nu är jag ingen expert på partikelfilter, som sagt bara kommit i kontakt med det under mina studier. Men jag skulle säga att dina pseudopunkter 1-4 är identiska för ett partikelfilter.
Partikelfiltret kan också beskrivas med en mätuppdatering och tidsuppdatering, precis som Kalman filtret.

Jag är ändå inte med på noterna.

Ett PF kräver inte mer mätdata än ett KF.

Bra.

Jag tror däremot att jag förstår vad det är som får dig till att tro annat så jag ska göra ett försök att förklara.

Vi kan börja med att jag mäter x,y,Z i ett rum. Jag vill veta vart jag befinner mig. Mina tre givare utsätts för ett non-gaussiskt brus.
Vad gör jag nu när jag har ett mätvärde som består utav en vektor som innehåller x,y,z?

När filtret sen kör så används inte KDE, KDE hör till förarbetet av implementationen.

Uppdateras inte modellen hela tiden? Eller antar man en fixerad modell?

Så i modelleringsarbetet kan det vara så att du får lägga ner mer energi för ett PF än ett KF, däremot kan du ju applicera PF på samma modell som i KF dvs gaussiskt processbrus, medelvärde 0 och kovarians Q. Då bör förarbetet vara densamma.

Jag har bara använt mig av olinjöra ODE:er för ett SR-UKF. Det är en bättre version än vanlig UKF då UKF har problem med ta roten ur covariansmatrisen.

Nu har ju partikelfiltret en hel del partiklar men man tar helt enkelt det skattade tillståndet, drar en massa slumpmässiga partiklar med hjälp av den modellen man har.
Man gör en prediktionen av dessa partiklar (som kan vara säg 1000 st) och när ett mätvärde kommer då skattar man sannolikheten varje partikel har för det mätvärdet och detta blir vikten. Sedan väljer man den partikeln med högst sannolikhet som sitt skattade tillstånd.
Sen predikterar man med sina partiklar än en gång och ett nytt mätvärde kommer in och man skattar på nytt sannolikheten för varje partikel osv. Däremot så har partiklarna en tendens till att dö ut, dvs de blir väldigt osannolika, därför brukar man resampla beroende på något kriterie, t ex en threshold på vikterna i kombination med antalet partiklar. När man anser att för många partiklar dött ut då slänger man de partiklarna man har och tar sin modell och genererar nya partiklar precis som i början men nu utgår allt från det nuvarande skattade tillståndet.

PF är mer beräkningstungt och det blir ju en hel del funktionsevalueringar av modellen då man måste hålla koll på alla partiklarna men partiklarna kommer inte från mätvärden utan de är genererade m.h.a modellen och man evaluerar allteftersom mätvärden kommer in hur sannolika varje partikel är och i varje tidssteg väljer den mest sannolika som sin tillståndsskattning (man skulle kunna se dom som parallella simuleringar av systemet).

Vi börjar från steg 1. Jag har ett mätdata från tre givare. Vad gör jag nu om jag vet att bruset är olinjärt och jag vill estimera mätdatat?

[rohol]

Måste ju börja med att tacka för frågorna, har ju fått fräscha upp kunskaperna lite, alltid roligt.

Modellen uppdateras inte hela tiden, med det sagt kan det säkerligen finnas tillämpningar som försöker sig på detta. (Ett sätt är väl att ta in modellparametrar som tillstånd och modellera parametrarna, men här ska vi inte glida iväg.)

Mätuppdatering: Ta tillvara på informationen i mätningen och uppdatera skattningen av tillstånden.
Tidsuppdatering: Prediktera var vi tror oss vara i nästa steg från nuvarande skattning av tillstånden.

Då det är dina givare som utsätts för ett non-gaussiskt brus så påverkas mätekvationen och felet kommer in i mätuppdateringen.
I detta steg har du redan en massa partiklar med dess tillhörande vikt där vikten bär på information om vad som hänt i tidigare iterationer, låg vikt => låg sannolikhet för att denna partikel skattar tillstånden bäst.
Du får in din mätning och du har dina partiklar, vad är sannolikheten för vardera partikel att generera ditt mätvärde med den brusmodellen du har i mätekvationen i kombinationen med vikterna som bär på äldre information också (Klurar på hur jag ska förklara detta men tänk dig att en partikel har haft 1 %, 5 %, 10 % sannolikheter vid de tre tidigare mätningarna, bara för att den nu vid fjärde mätningen har 95 % sannolikhet så är den ju kanske inte trolig då: 0.01*0.05*0.1*0.95 = 0.00475 %. Är det första iterationen eller att man resamplat för att partiklarna dött ut och vikterna blivit små så sätter man ofta att sannolikheten för vardera partikel är lika stor och första valet blir bara den med högst sannolikhet mot nuvarande mätvärde.).
Din skattning av nuvarande tillstånd med den mätningen du gjort så väljer du helt enkelt den partikel med högst vikt efter att du uppdatera vikterna med senaste sannolikhetsutfallet.

Letade rätt på vad jag tror är orginal pappret av partikelfiltret (även kallat bootstrap filter).
https://www.ece.iastate.edu/~namrata/EE ... proach.pdf

Pappret i sig är kort men verkar välskrivet.
Är du nyfiken så skulle jag rekommendera att du själv återskapar exemplena i avsnitt 4, all information som behövs för detta bör finnas i pappret.

[DanielM]

Okej!

Vi säger så här. Innan jag börjar mäta med mitt filter, då skapar jag min modell.

Steg 1:
Jag börjar med att logga X antal godtyckliga mätvärden.

Steg 2:
Där efter väljer jag en normalfördelad gaussisk fördelningskruva som har medelvärdet av X och standardavvikelsen av X.

Steg 3:
Jag skapar olika vikter W för min gaussiska fördelningskruva igenom att stoppa in mina godtyckliga värden X i den gaussiska fördelningskurvan, som ger mig en sannolikhet tillbaka. Vi kalla denna sannolikhet för S1. För att hitta vikterna W så måste jag hitta S2, vilket är sannolikheten för ett histogram av X.

Nu delar jag S2 / S1 för att hitta vikterna W. Detta betyder att jag delar sannolikhetsförhållandet mellan histogrammet av X mo den gaussiska fördelningskurvan för att kolla hur mycket dom skiljer med varandra.

Nu har jag massvis med vikter W som jag kan använda för att forma en gaussisk fördelningskurva som har medelvärdet av X och standardavvikelsen av X.

Steg 4:
Jag testar med ett mätvärde x. Jag räknar ut sannolikheten för mitt mätvärde x igenom att använda min gaussiska fördelningskruva som har medelvärdet av X och standardavvikelsen av X.
Men här plockar jag fram vikten w (som representerar för ett visst mätvärde från vikterna W) och multiplicerar vikten w med sannolikhetsvärdet som jag fick från min gaussiska fördelningskurva, dvs min modell.

Nu har jag fått en verkliga sannolikheten.

Frågor:
1. Blir jag inte låst till att min modell har ett fixerat medelvärde? Tänk om jag skapar en modell med mätvärdena 80 till 120, men senare mäter jag vid -5?
2. Hur vet jag vilken vikt jag ska använda? Kan man t.ex. säga: Om jag har mätvärdet mellan 20 och 23, då ska jag använda vikt nummer 4 som jag multiplicerar med min gaussiska fördelningskurva?

[rohol]

DanielM, förstår inte riktigt vad du avser att stegen är.

1. Blir jag inte låst till att min modell har ett fixerat medelvärde? Tänk om jag skapar en modell med mätvärdena 80 till 120, men senare mäter jag vid -5?

Om du väljer att ha en modell med fixerat medelvärde så blir du ju det, finns ju däremot inget som hindrar dig från att ha en annan modell som även beror på eventuella tillstånd och externa signaler så som styrsignal.
En vanlig struktur är ju att man har:
x[k+1] = f(x[k], u[k]) + v[k]
där bruset, v, t ex modelleras som Normalfördelat, N(mu, sigma^2).
E[ x[k+1] ] = f(x[k], u[k]) + mu, där E här är expected value / väntevärde.

2. Hur vet jag vilken vikt jag ska använda? Kan man t.ex. säga: Om jag har mätvärdet mellan 20 och 23, då ska jag använda vikt nummer 4 som jag multiplicerar med min gaussiska fördelningskurva?

Här förstår jag tyvärr inte frågan, det enklaste är väl att initiera det med att alla partiklars vikter är 1/N, där N är antalet partiklar.


För varje partikel i varje uppdatering så drar man ett värde för v i modellen ovan från dess fördelning.

[DanielM]

DanielM, förstår inte riktigt vad du avser att stegen är.

Jag vet inte heller.
Jag hittar olika matematiska tillämpningar på partikelfilter, så det kanske inte finns just en generell definition på partikelfilter, utan man gör det som fungerar?

Om du väljer att ha en modell med fixerat medelvärde så blir du ju det, finns ju däremot inget som hindrar dig från att ha en annan modell som även beror på eventuella tillstånd och externa signaler så som styrsignal.
En vanlig struktur är ju att man har:
x[k+1] = f(x[k], u[k]) + v[k]
där bruset, v, t ex modelleras som Normalfördelat, N(mu, sigma^2).
E[ x[k+1] ] = f(x[k], u[k]) + mu, där E här är expected value / väntevärde.

Skulle jag kunna använda en dold markovmodell istället för
\(x[k+1] = f(x[k], u[k])\)

I så fall kan jag använda den i mitt SR-UKF filter istället för att bygga ett partikelfilter. För både SR-UKF och PF gör ju exakt samma sak - Estimera tillståndet.

[DanielM]

Vi kan ta det från början.

Om jag har en tillståndsvektor X[k] som jag vill estimera. X[k] kommer från en eller flera givare. Vart ska jag börja då? Hur kan jag estimera X[k]?

[DanielM]

Rätta mig om jag har gjort fel.

Men jag tror jag har lyckats göra ett partikelfilter...på mitt speciella lilla sätt.
Jag har tittat på hur partikelfilter fungerar och jag är inte alls imponerad över deras konfiguration. Man måste alltså konfigurera ett filter för att få det bra. Som vanligt så gillar jag att förenkla samtidigt som jag optimerar. Så här är mitt bidrag på mitt partikelfilter.

Kod: Markera allt

function y = pf(varargin)
  % Check if there is any input
  if(isempty(varargin))
    error('Missing inputs')
  end

  % Get input
  if(length(varargin) >= 1)
    x = varargin{1};
  else
    error('Missing input')
  end

  % Get prediction horizon
  if(length(varargin) >= 1)
    p = varargin{2};
  else
    error('Missing prediction horizon')
  end

  % Get the size
  [m, n] = size(x);

  % Create horizon
  horizon = zeros(m, p);

  % Create the filtration
  y = zeros(m, n);

  % Loop all rows of x at the same time
  j = 1;
  k = 1;
  while(j <= n)
    % Horizon shifting
    for i = 1:m
      % Add or shift the horizon
      if(k >= p)
        % Shift the horizon with -1 step
        horizon = circshift(horizon, 1);

        % Add the last
        horizon(i, p) = x(i, j);

      else
        % Add to the horizon
        horizon(i, k) = x(i, j);

      end
    end

    % Compute the kernel density function from the horizon
    [P, H] = kernel_density_estimation(horizon, m, p);

    % Estimate the next value
    for i = 1:m
      % Find the corresponding index of x and sorted H
      absolute_values = abs(x(i, j) - H(i, :));
      [~, index] = min(absolute_values);

      % Compute the ratio (0.5-1.0)
      % If P(i, index) = 1 (100%), then ratio = 0.5 (50%)
      % If P(i, index) = 0 (0%), then ratio = 1.0 (100%)
      ratio = x(i, j)/(x(i, j) + x(i, j) * P(i, index));

      % Difference between old and new
      if(j > 1)
        diff = x(i, j) - x(i, j-1);
      else
        diff = 0;
      end

      % State update. It MUST be negative
      y(i, j) = x(i, j) - ratio*diff;

    end

    % Next measurement column
    j = j + 1;

    % Next horizon column
    if(k < p)
      k = k + 1;
    end
  end

end



function [P, H] = kernel_density_estimation(x, m, n)
  % Sort x so smallest values first because H is like a histogram with only one count for each value
  H = sort(x, 2, 'ascend');

  % Create empty array
  P = zeros(m, n);

  % Standard distribution is 1
  sigma = 1;

  % Loop every row of P
  for i = 1:m
    % Get the whole row
    h = H(i, :);

    % Loop every column of H
    for j = 1:n
      % Assume that the j:th value of h(j) is the mean
      mu = h(j);

      % Compute the propability of every values of h and add them to P
      P = P + normal_pdf(h, mu, sigma);
    end
  end

  % Turn P into a probability where the largest number is 1.0 (100%)
  P = P/max(P);
end

function y = normal_pdf(x, mu, sigma)
  % Normal distribution
  y = 1/(sigma*sqrt(2*pi))*e.^(-1/2*(x-mu).^2/sigma^2);
end

Teorin är väldigt enkel.
För varje värde som jag har så uppdaterar jag en olinjär sannolikhetskruva som modell. Den liknar väldigt mycket normalfördelning om det är normalfördelat brus.
För att skapa modellen så använder jag KDE - Kernel Density Estimation. Den använder sig av en normalfördelat sannolikhetskruva som kärna (kernel).

Se film här:


Men skillnaden är att jag skapar typ ett histogram där högsta värdet är 1, vilket betyder att det är det mest vanligaste värdet.
Om jag har ett mätvärde X som stämmer överens med histogrammet till 100%. Då lägger jag mig i mitten av någon av sidorna, beroende på om skillnaden mellan föregående och nuvarande mätvärde är negativ eller positiv.

Så skillnad mellan ett vanligt filter och ett partikelfilter så estimerar detta filtret grundat på tidigare händelser, dvs gissar vad nästa värde (+1) kan vara grundat på statistik. Den kan alltså inte prediktera +2 eller mer steg i framtiden. Då måste jag nog använda mig av något annat.

Men detta filter anropas med två argument, märvärde och hur stor kernel-density-estimation modellen ska vara.

Skärmbild 2022-10-22 171710.png

Ladda ned programmet GNU Octave och skapa en fil som heter pf.m.
Ni använder kommandot i terminalen (Command Window)

Kod: Markera allt

edit pf.m

Sedan klistrar ni in koden som finns ovan. Klart!
För att köra programmet så skapar ni en liten signal, typ en lång lista som jag kallar för y. Så här ser det ut i min terminal:

Kod: Markera allt

>> y; % Measurements
>> yn = y + 0.5*randn(1, length(y)); % Add noise
>> p = 20; % Intrimmingsparameter
>> yf = pf(yn, p); % Filter
>> plot(t, yf, t, yn) % Plot
>> legend('filter', 'noise', 'FontSize', 20)
>> grid on

Skärmbild 2022-10-22 172911.png

Här är ett annat exempel:

Kod: Markera allt

>> t = linspace(0, 2*pi*10);
>> y = sin(t);
>> t = linspace(0, 2*pi*10, 1000);
>> y = sin(t);
>> yn = y + 0.1*randn(1, length(y)); % Add noise
>> p = 100; % Intrimmingsparameter
>> yf = pf(yn, p); % Filter
>> plot(t, yf, t, yn) % Plot
>> grid on
>> legend('filter', 'noise', 'FontSize', 20)

Skärmbild 2022-10-22 202902.png

Om jag återfiltrerar min filterade signal så får jag finare signal, men fasförskjutning. Vilket tyder på att allt är i sin ordning.

Kod: Markera allt

>> yf = pf(yn, 15); % Filter
>> yf = pf(yf, 15); % Filter
>> yf = pf(yf, 15); % Filter
>> yf = pf(yf, 15); % Filter
>> yf = pf(yf, 15); % Filter
>> yf = pf(yf, 15); % Filter
>> plot(t, yf, t, yn) % Plot

Skärmbild 2022-10-22 203636.png

Edit:
Nu har jag förbättrat skriptet så man behöver bara köra 1 gång för att det ska bli bra.
Kör man fler än 1 gång så får man fasförskjutning.

Kod: Markera allt

function y = pf(varargin)
  % Check if there is any input
  if(isempty(varargin))
    error('Missing inputs')
  end

  % Get input
  if(length(varargin) >= 1)
    x = varargin{1};
  else
    error('Missing input')
  end

  % Get prediction horizon
  if(length(varargin) >= 2)
    p = varargin{2};
  else
    error('Missing prediction horizon')
  end

  % Get the size
  [m, n] = size(x);

  % Create horizon
  horizon = zeros(m, p);

  % Create the filtration
  y = zeros(m, n);

  % Loop all rows of x at the same time
  j = 1;
  k = 1;
  while(j <= n)
    % Horizon shifting
    horizon = shift_horizon(horizon, x, p, k, i, j, m);

    % Compute the kernel density function from the horizon
    [P, H] = kernel_density_estimation(horizon, m, p);

    % Estimate the next value
    for i = 1:m
      for a = 1:2
        % Find the corresponding index of x and sorted H
        absolute_values = abs(x(i, j) - H(i, :));
        [~, index] = min(absolute_values);

        % Compute the ratio (0.5-1.0)
        % If P(i, index) = 1 (100%), then ratio = 0.5 (50%)
        % If P(i, index) = 0 (0%), then ratio = 1.0 (100%)
        ratio = x(i, j)/(x(i, j) + x(i, j) * P(i, index));

        % Difference between old and new
        if(j > 1)
          diff = x(i, j) - x(i, j-1);
        else
          diff = 0;
        end

        % Update state. It MUST be negative
        switch(a)
        case 1
          x(i, j) = x(i, j) - ratio*diff; % Re-estimate
        case 2
          y(i, j) = x(i, j) - ratio*diff; % Output
        end
      end
    end

    % Next measurement column
    j = j + 1;

    % Next horizon column
    if(k < p)
      k = k + 1;
    end
  end

end


function horizon = shift_horizon(horizon, x, p, k, i, j, m)
  % Horizon shifting
  for i = 1:m
    % Shift
    if(k >= p)
      % Shift the horizon with -1 step
      horizon(i, 1:p-1) = horizon(i, 2:p);
    end

    % Add the last
    horizon(i, k) = x(i, j);
  end
end



function [P, H] = kernel_density_estimation(x, m, n)
  % Sort x so smallest values first because H is like a histogram with only one count for each value
  H = sort(x, 2, 'ascend');

  % Create empty array
  P = zeros(m, n);

  % Standard distribution is 1
  sigma = 1;

  % Loop every row of P
  for i = 1:m
    % Get the whole row
    h = H(i, :);

    % Loop every column of H
    for j = 1:n
      % Assume that the j:th value of h(j) is the mean
      mu = h(j);

      % Compute the propability of every values of h and add them to P
      P = P + normal_pdf(h, mu, sigma);
    end
  end

  % Turn P into a probability where the largest number is 1.0 (100%)
  P = P/max(P);
end

function y = normal_pdf(x, mu, sigma)
  % Normal distribution
  y = 1/(sigma*sqrt(2*pi))*e.^(-1/2*(x-mu).^2/sigma^2);
end

Resultat:

Kod: Markera allt

>> yf = pf(yn, 100); % Filter
>> plot(t, yf, t, yn) % Plot
>> grid on
>> legend('filter', 'noise', 'FontSize', 20)

Skärmbild 2022-10-22 212205.png

Bruset är även olinjärt

Skärmbild 2022-10-23 005840.png

[DanielM]

Här är den rekursiva versionen. Ett partikelfilter har likheter med ett kalmanfilter. Den estimerar (gissar) nästa mätvärde. Så den är alltid +1 steg i framtiden. Skillnaden mellan ett kalmanfilter och partikelfilter är modellen. Ett kalmanfilter använder en differentials-ekvation som dynamisk modell, medan ett partikelfilter använder en fördelningskurva som modell t.ex. normalfördelningskurvan. Fördelen med ett partikelfilter är att behöver inte specificera vilken modell man använder då partikelfiltret själv skapar modellen igenom kernel density estimation, som kan även vara anpassad för olinjära brussignaler. Något som finns ej möjlighet med ett kalmanfilter då ett kalmanfilter förmodar att bruset är normalt fördelat, medan partikelfiltret antar att bruset är olinjärt fördelat .

Exempel:

Kod: Markera allt

% Create inputs
N = 200;
u = linspace(1, 1, N);
u = [5*u 10*u -4*u 3*u 5*u 0*u -5*u 0*u];

% Create time
t = linspace(0, 100, length(u));

% Create second order model
G = tf(1, [1 0.8 3]);

% Simulate outputs
y = lsim(G, u, t);
close

% Add noise
e = 0.1*randn(1, length(u));
y = y + e;

% Do particle filtering - Tuning parameters
p = 30;                            % Length of the horizon (Change this)

% Particle filter - No tuning
[m, n] = size(y);                  % Dimension of the output state and length n
yf = zeros(m, n);                  % Filtered outputs
horizon = zeros(m, p);             % Horizon matrix
xp = zeros(m, 1);                  % Past state
k = 1;                             % Horizon counting (will be counted to p. Do not change this)
noise = rand(m, p);                % Random noise, not normal distributed

% Particle filter - Simulation
for i = 1:n
  x = y(:, i);                     % Get the state
  [xhat, horizon, k, noise] = pf_recursive(x, xp, k, horizon, noise);
  yf(:, i) = xhat;                 % Estimated state
  xp = xhat;                       % This is the past state
end

% Plot restult
plot(t, y, t, yf, '-r')
grid on

Partikelfilter

Kod: Markera allt

function [xhat, horizon, k, noise] = pf_recursive(varargin)
  % Check if there is any input
  if(isempty(varargin))
    error('Missing inputs')
  end

  % Get state input
  if(length(varargin) >= 1)
    x = varargin{1};
  else
    error('Missing state input')
  end

  % Get past state input
  if(length(varargin) >= 2)
    xp = varargin{2};
  else
    error('Missing past state input')
  end

  % Get horizon index
  if(length(varargin) >= 3)
    k = varargin{3};
  else
    error('Missing horizon index')
  end

  % Get horizon
  if(length(varargin) >= 4)
    horizon = varargin{4};
  else
    error('Missing horizon')
  end

  % Get noise matrix
  if(length(varargin) >= 5)
    noise = varargin{5};
  else
    error('Missing noise matrix')
  end

  % Get state dimension m
  m = size(x, 1);

  % Create the estimated state
  xhat = zeros(m, 1);

  % Get length of horizon
  p = size(horizon, 2);

  % Horizon matrix shifting
  horizon = shift_matrix(horizon, x, p, k, m);

  % Compute the kernel density function from the horizon
  [P, H] = kernel_density_estimation(horizon, m, p, noise);

  % Estimate the next value
  for i = 1:m
    % Find the corresponding index of x and sorted H
    absolute_values = abs(x(i) - H(i, :));
    [~, index] = min(absolute_values);

    % Compute the ratio (0.5-1.0)
    % If P(i, index) = 1 (100%), then ratio = 0.5 (Good)
    % If P(i, index) = 0 (0%), then ratio = 1.0 (Problem)
    ratio = x(i)/(x(i) + x(i) * P(i, index));

    % Difference between old and new
    diff = x(i) - xp(i);

    % This gives a smoother filtering
    horizon(i, k) = horizon(i, k)*abs(diff);

    % Update state. It MUST be negative
    xhat(i) = x(i) - ratio*diff;

    % Compute the noise
    e = xhat(i) - x(i);

    % Noise matrix shifting
    noise = shift_matrix(noise, e, p, k, m);
  end

  % Next matrix column
  if(k < p)
    k = k + 1;
  end
end

function matrix = shift_matrix(matrix, x, p, k, m)
  % Matrix shifting
  for i = 1:m
    % Shift
    if(k >= p)
      % Shift the matrix with -1 step
      matrix(i, 1:p-1) = matrix(i, 2:p);
    end

    % Add the last
    matrix(i, k) = x(i);
  end
end

function [P, H] = kernel_density_estimation(x, m, n, noise)
  % Sort x so smallest values first because H is like a histogram with only one count for each value
  H = sort(x, 2, 'ascend');

  % Create empty array
  P = zeros(m, n);

  % Loop every row of P
  for i = 1:m
    % Get the whole row
    h = H(i, :);

    % Assume that the sigma is the standard deviation of the noise
    sigma = std(noise(i, :));

    % Loop every column of H
    for j = 1:n
      % Assume that the j:th value of h(j) is the mean
      mu = h(j);

      % Compute the propability of every values of h and add them to P
      P(i, :) = P(i, :) + normal_pdf(h, mu, sigma);
    end
  end

  % Turn P into a probability where the largest number is 1.0 (100%)
  for i = 1:m
    P(i, :) = P(i, :) / max(P(i, :));
  end
end

function y = normal_pdf(x, mu, sigma)
  % Normal distribution
  y = 1/(sigma*sqrt(2*pi))*e.^(-1/2*(x-mu).^2/sigma^2);
end

Resultat:

Skärmbild 2022-10-23 194417.png