Implementera ett UKF filter på inbyggda system?

PIC, AVR, Arduino, Raspberry Pi, Basic Stamp, PLC mm.
DanielM
Inlägg: 2312
Blev medlem: 5 september 2019, 14:19:58

Implementera ett UKF filter på inbyggda system?

Inlägg av DanielM »

Jag har länge varit behov utav en filtrering som är generell och passar alla typer av system.
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.
Skärmklipp.PNG
Problemet verklig mätning är att medelvärde och spridningen varierar över tid.
Mean.png
Std.png
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.
Skärmklipp.PNG
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
log.txt

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
Du har inte behörighet att öppna de filer som bifogats till detta inlägg.
pfyra
Inlägg: 363
Blev medlem: 8 mars 2015, 14:14:44
Ort: utanför Karlstad

Re: Implementera ett UKF filter på inbyggda system?

Inlägg av pfyra »

Vad har du för mål? Behövs filtrering för att kunna nå detta mål? I så fall har du nog svaret på om det är värt att implementera filtret.
Rick81
Inlägg: 746
Blev medlem: 30 december 2005, 13:07:09

Re: Implementera ett UKF filter på inbyggda system?

Inlägg av Rick81 »

Jag har använt både glidande medelvärde och kalmanfilter på liknande data i inbyggda system.

Det är största skillnaden är att kalmanfilter jobbar i flera dimensioner medan glidande medelvärd blir en dimension. Sen är det mycket mer jobb implementera ett kalmanfilter.

Som vanligt beror det på vad det är för data du ska filtrera. Är ex. det bara en analog spänning som ska jämnas ut funkar glidande medelvärde utmärkt. Ska man ha en självgående robot är kalmanfiltet att föredra.

Sen skulle jag säga att kalman filter bör jobba i float så har man inte fpu kan detta påverka valet.

Ser ger ju Kalman bättre filtrering/respons men frågan om det är värt det beror ju på datakvalite/krav/applikationen
DanielM
Inlägg: 2312
Blev medlem: 5 september 2019, 14:19:58

Re: Implementera ett UKF filter på inbyggda system?

Inlägg av DanielM »

pfyra skrev: 26 juni 2021, 23:06:06 Vad har du för mål? Behövs filtrering för att kunna nå detta mål? I så fall har du nog svaret på om det är värt att implementera filtret.
Målet är att få till med en bra filtrering. Jag finner glindade medelvärde inte så effektivt.
Rick81 skrev: 27 juni 2021, 10:26:47 Jag har använt både glidande medelvärde och kalmanfilter på liknande data i inbyggda system.

Det är största skillnaden är att kalmanfilter jobbar i flera dimensioner medan glidande medelvärd blir en dimension. Sen är det mycket mer jobb implementera ett kalmanfilter.

Som vanligt beror det på vad det är för data du ska filtrera. Är ex. det bara en analog spänning som ska jämnas ut funkar glidande medelvärde utmärkt. Ska man ha en självgående robot är kalmanfiltet att föredra.

Sen skulle jag säga att kalman filter bör jobba i float så har man inte fpu kan detta påverka valet.

Ser ger ju Kalman bättre filtrering/respons men frågan om det är värt det beror ju på datakvalite/krav/applikationen
Jag körde ovan bara 1 dimension på UKF.

Om du ser ovan så verkar glidande medelvärde vara sämre än UKF.

Jag skapade nyss ett kalman filter i C-kod.

Kod: Markera allt

/*
 * Compute the mean of vector x
 * x[L] Vector with values
 * L = Length of vector x
 */
float mean(float* x, uint32_t length) {
	float s = 0;
	for(uint32_t i = 0; i < length; i++)
		s += *(x + i);
	return s/((float) length);
}


/*
 * Compute Standard deviation
 * x[L] Vector with values
 * L = Length of vector x
 */
float std(float* x, uint32_t length) {
	float mu = mean(x, length);
	float sigma = 0;
	for(uint32_t i = 0; i < length; i++)
		sigma += (*(x + i) - mu) * (*(x + i) - mu);
	return sqrtf(sigma/((float) length));
}

int main() {

	// Initial parameters
	uint8_t L = 1; 				// How many states we have
	float a = 0.5; 				// Alpha value
	float b = 2; 				// Beta value
	float k = 1; 				// Kappa value
	float Q[1 * 1] = { 0.0001 }; 	// Initial disturbance covariance matrix
	float R[1 * 1] = { 2 }; 		// Initial noise covariance matrix
	float P[1 * 1] = { 1 }; 		// Initial covariance matrix
	float xhat[1] = { 15 }; // Initial estimated state, a rule of thumb: Use a regular measurement only
	float zk[1] = { 0 };			// This is our measurement
	float u[1] = { 0 }; // u is not used in this example due to the transition function not using a input signal

	// Our transition function is a random variable
	void ukf_transition(float *x, float *s, float *u, uint8_t L) {
		uint8_t N = 2 * L + 1;
		float a[L];
		float r[1];
		for (uint8_t j = 0; j < N; j++) {
			for (uint8_t i = 0; i < L; i++)
				*(a + i) = *(s + i * N + j); // Extract one column from matrix s
			randn(r, 1, 0, 1); // Create one random value with mean 0 and deviation 1
			*(x + j) = std(a, L) * *r + mean(a, L);
		}
	}

	// Data of a laser measurement with mean 14 and deviation 1
	float X[500]; // Estimated
	float data[500] = { 15, 14, 13, 14, 15, 14, 16, 14, 15, 14, 13, 15, 14, 15,
			13, 14, 16, 14, 15, 14, 15, 14, 15, 14, 15, 14, 15, 16, 15, 14, 13,
			14, 15, 13, 14, 13, 14, 15, 14, 15, 13, 15, 14, 16, 14, 13, 14, 15,
			13, 15, 14, 15, 13, 14, 15, 13, 15, 14, 13, 14, 15, 16, 13, 15, 14,
			13, 15, 14, 16, 14, 16, 15, 17, 14, 16, 14, 13, 16, 13, 15, 16, 15,
			14, 15, 14, 12, 15, 13, 14, 13, 12, 15, 13, 12, 14, 15, 14, 13, 14,
			15, 14, 13, 16, 15, 13, 14, 15, 14, 15, 12, 16, 15, 17, 15, 14, 13,
			14, 15, 13, 14, 16, 14, 15, 14, 15, 13, 15, 14, 15, 13, 15, 13, 15,
			16, 13, 14, 15, 16, 14, 13, 15, 13, 16, 15, 13, 15, 16, 15, 14, 15,
			13, 14, 15, 14, 13, 15, 14, 15, 14, 13, 14, 15, 14, 15, 16, 15, 14,
			15, 14, 15, 14, 16, 14, 13, 14, 15, 14, 15, 14, 15, 14, 15, 14, 13,
			14, 15, 14, 13, 14, 17, 14, 15, 14, 15, 13, 14, 13, 14, 16, 14, 15,
			14, 13, 15, 13, 12, 13, 14, 15, 14, 15, 13, 14, 15, 14, 13, 14, 16,
			14, 13, 14, 15, 16, 13, 14, 15, 13, 14, 13, 15, 14, 15, 16, 13, 14,
			13, 14, 13, 14, 12, 15, 14, 15, 16, 14, 13, 15, 14, 15, 14, 15, 14,
			13, 15, 13, 14, 13, 14, 15, 14, 15, 14, 15, 13, 14, 15, 13, 14, 16,
			14, 15, 14, 13, 15, 14, 15, 14, 16, 15, 13, 16, 15, 13, 15, 14, 15,
			16, 14, 15, 13, 14, 15, 14, 15, 12, 15, 13, 14, 16, 15, 13, 14, 15,
			14, 15, 14, 15, 13, 14, 15, 14, 15, 14, 13, 15, 12, 14, 15, 13, 15,
			12, 15, 14, 15, 14, 13, 15, 14, 15, 13, 14, 15, 14, 15, 12, 13, 15,
			14, 15, 14, 13, 14, 15, 13, 16, 15, 14, 15, 14, 15, 13, 14, 15, 16,
			15, 14, 15, 14, 15, 14, 15, 14, 16, 13, 15, 14, 16, 15, 14, 13, 14,
			13, 14, 15, 14, 15, 14, 15, 14, 15, 13, 16, 13, 14, 13, 14, 15, 13,
			15, 14, 16, 14, 15, 14, 15, 13, 15, 13, 14, 15, 14, 15, 14, 13, 14,
			13, 14, 15, 13, 14, 15, 14, 15, 14, 15, 14, 15, 14, 15, 13, 15, 14,
			15, 16, 14, 15, 14, 12, 14, 16, 14, 15, 14, 15, 14, 15, 13, 14, 13,
			14, 12, 14, 13, 14, 15, 14, 15, 14, 15, 13, 15, 13, 16, 14, 15, 14,
			15, 14, 15, 14, 15, 16, 14, 15, 13, 15, 13, 16, 14, 13, 14, 15, 13,
			15, 13, 16, 15, 12, 14, 13, 16, 15, 14, 13, 14, 15, 17, 15, 14, 13,
			14, 16, 15, 14, 13, 15, 14, 16, 15, 14 };

	// Do UKF
	for (uint32_t i = 0; i < 500; i++) {
		zk[0] = data[i];
		X[i] = xhat[0];
		ukf(xhat, zk, u, P, Q, R, a, k, b, L, ukf_transition);
	}
}

Här är UKF koden.

Kod: Markera allt

static void ukf_create_weights(float *Wa, float *Wc, float a, float b, float k, uint8_t L);
static void ukf_compute_sigma_points(float *s, float *x, float *P, float a, float k, uint8_t L);
static void ukf_multiply_weights(float *x, float *xi, float *W, uint8_t L);
static void ukf_estimate_covariance(float *P, float *xi, float *x, float *W, float *O, uint8_t L);
static void ukf_estimate_cross_covariance(float* Csz, float* s, float* xhat, float* z, float* zhat, float* Wc, uint8_t L);
static void ukf_create_kalman_K(float *K, float *Shat, float *Csz, uint8_t L);
static void ukf_state_update(float *K, float *Shat, float *P, float *xhat, float *zk, float *zhat, uint8_t L);
static void chol(float *A, float *L, int row);
static void mul(float* A, float* B, float* C, int row_a, int column_a, int column_b);

/*
 * Unscented Kalman Filter
 * xhat[L] = Estimated state vector
 * zk[L] = Real world measurement vector
 * u[L] = Input signal
 * P[L*L] = Covariance matrix
 * Q[L*L] = Disturbance covariance matrix
 * R[L*L] = Noise covaraiance matrix
 * a = Alpha number, a small number, but not very small due to float presition
 * k = Kappa number, usually zero.
 * b = Beta number, 2 = optimal for gaussian distribution
 * L = Number of states, or sensors in practice.
 * This follows wikipedia article: https://en.wikipedia.org/wiki/Kalman_filter#Unscented_Kalman_filter
 */
void ukf(float* xhat, float* zk, float* u, float* P, float* Q, float* R, float a, float k, float b,  uint8_t L, void (*ukf_transition)(float*, float*, float*, uint8_t)) {
	// Column
	uint8_t N = 2 * L + 1;

	// Init - Create the weights
	float Wa[N];
	float Wc[N];
	ukf_create_weights(Wa, Wc, a, b, k, L);

	// PREDICT: Step 0 - Predict the state and state estimation error covariance at the next time step
	float s[L*N];
	ukf_compute_sigma_points(s, xhat, P, a, k, L);

	// PREDICT: Step 1 - Run our transition function
	float x[L*N];
	ukf_transition(x, s, u, L);

	// PREDICT: Step 2 - Combine the predicted states to obtain the predicted states
	ukf_multiply_weights(xhat, x, Wa, L);

	// PREDICT: Step 3 - Compute the covariance of the predicted state
	ukf_estimate_covariance(P, x, xhat, Wc, Q, L);

	// UPDATE: Step 1 - Use the nonlinear measurement function to compute the predicted measurements for each of the sigma points.
	float z[L*N];
	memcpy(z, s, L * N * sizeof(float)); // Here we assume that the observation function z = h(s, u) = s

	// UPDATE: Step 2 - Combine the predicted measurements to obtain the predicted measurement
	float zhat[L];
	ukf_multiply_weights(zhat, z, Wa, L);

	// UPDATE: Step 3 - Estimate the covariance of the predicted measurement
	float Shat[L*L];
	ukf_estimate_covariance(Shat, z, zhat, Wc, R, L);

	// UPDATE: Step 4 - Estimate the cross-covariance
	float Csz[L*L];
	ukf_estimate_cross_covariance(Csz, s, xhat, z, zhat, Wc, L);

	// UPDATE: Step 5 - Find kalman K matrix
	float K[L*L];
	ukf_create_kalman_K(K, Shat, Csz, L);

	// UPDATE: Step 6 - Obtain the estimated state and state estimation error covariance
	ukf_state_update(K, Shat, P, xhat, zk, zhat, L);
}

/*
 * Create weight vector
 * Wa[2*L + 1] = Weight vector for covariance sigma points
 * Wc[2*L + 1] = Weight vector for covariance matrices
 * a = A small number
 * b = Knowledge about the gaussian distirbution. b = 2 (Optimal for gaussian distribution)
 * k = Usually set to 0
 * L = how many states we have
 */
static void ukf_create_weights(float *Wa, float *Wc, float a, float b, float k, uint8_t L) {
	uint8_t N = 2 * L + 1;
	for (uint8_t i = 0; i < N; i++){
		if (i == 0){
			*Wa = (a*a*k-L)/(a*a*k);
			*Wc = *Wa + 1 - a*a + b;
		}else{
			*(Wa + i) = 1/(2*a*a*k);
			*(Wc + i) = *(Wa + i);
		}
	}
}

/*
 * Multiply and sum the weights w with vector
 * x[L] = The output vector
 * xi[L*(2*L+1)] = Vector of sigma points
 * W[2*L+1] = Weight vector
 * L = Dimension of the vectors
 */
static void ukf_multiply_weights(float *x, float *xi, float *W, uint8_t L) {
	uint8_t N = 2 * L + 1;
	memset(x, 0, L * sizeof(float));
	for (uint8_t j = 0; j < N; j++)
		for (uint8_t i = 0; i < L; i++)
			*(x + i) += *(W + j) * *(xi + i * N + j);
}

/*
 * Compute the sigma points
 * s[L*(2*L + 1)] = Sigma point matrix
 * x[L] = Estimated vector
 * P[L*L] = Covariance matrix
 * a = Alpha factor
 * k = Kappa factor
 * L = Dimension of the vectors
 */
static void ukf_compute_sigma_points(float *s, float *x, float *P, float a, float k, uint8_t L) {
	uint8_t N = 2 * L + 1;
	uint8_t compensate_column = L + 1;

	// Compute A = a*sqrt(k)*chol(P)
	float c = a*sqrtf(k);
	float A[L * L];
	chol(P, A, L);
	for(uint8_t i = 0; i < L * L; i++)
		*(A + i) = c * *(A + i); // A = c*A
	for (uint8_t j = 0; j < N; j++)
		if (j == 0)
			for (uint8_t i = 0; i < L; i++)
				*(s + i * N + j) = *(x + i); 					    // First sigma vector will become as the previous estimated state
		else if (j >= 1 && j <= L)
			for (uint8_t i = 0; i < L; i++)
				*(s + i * N + j) = *(x + i) + *(A + i * L + j - 1); 		    // We take the j:th column of A from 0..L-1 where j >= 1
		else
			for (uint8_t i = 0; i < L; i++)
				*(s + i * N + j) = *(x + i) - *(A + i * L + j - compensate_column); // Same here. A have not the same amount of columns as s
}

/*
 * Estimate the covariance
 * P[L*L] = Covariance matrix
 * xi[L*(2*L + 1)] = Sigma point matrix
 * x[L] = Estimated vector
 * W[2*L + 1] = Weight vector
 * O[L*L] = Tuning matrix
 * L = Dimension of the vectors
 */
static void ukf_estimate_covariance(float *P, float *xi, float *x, float *W, float *O, uint8_t L) {
	uint8_t N = 2 * L + 1;
	float diff[L];
	float diffT[L];
	float diff_diffT[L * L];
	memset(P, 0, L * L * sizeof(float));
	for (uint8_t j = 0; j < N; j++) {
		for (uint8_t i = 0; i < L; i++)
			*(diff + i) = *(xi + i * N + j) - *(x + i); // Create the difference vector xi - x
		memcpy(diffT, diff, L * sizeof(float)); // We need to transpose difference vector as well
		mul(diff, diffT, diff_diffT, L, 1, L); // Create the matrix diff_diffT
		for (uint8_t i = 0; i < L * L; i++)
			*(P + i) += *(W + j) * *(diff_diffT + i); // P = sum(W*diff*diffT)
	}
	for (uint8_t i = 0; i < L * L; i++)
		*(P + i) += *(O + i); // Add the O matrix, which can be R or Q matrix
}

/*
 * Estimate the cross covariance
 * Csz[M*M] = Cross covariance matrix
 * s[L*(2*L + 1)] = Sigma point matrix
 * xhat[L] = Estimated vector
 * z[L*(2*L + 1)] = Sigma point matrix
 * zhat[L] = Estimated vector
 * Wc = Weight vector
 * L = Dimension of the vectors
 */
static void ukf_estimate_cross_covariance(float* Csz, float* s, float* xhat, float* z, float* zhat, float* Wc, uint8_t L) {
	uint8_t N = 2 * L + 1;
	float diffx[L];
	float diffyT[L];
	float diffx_diffyT[L * L];
	memset(Csz, 0, L * L * sizeof(float));
	for (uint8_t j = 1; j < N; j++) {
		for (uint8_t i = 0; i < L; i++) {
			*(diffx + i) = *(s + i * N + j) - *(xhat + i);  // Create the difference vector s - xhat
			*(diffyT + i) = *(z + i * N + j) - *(zhat + i); // Create the difference vector z - zhat
		}
		mul(diffx, diffyT, diffx_diffyT, L, 1, L); // Create the matrix diffx_diffyT
		for (uint8_t i = 0; i < L * L; i++)
			*(Csz + i) += *(Wc + j) * *(diffx_diffyT + i); // Sum P = sum(Wc*diffx*diffyT)
	}
}

/*
 * Create kalman K matrix
 * K[L*L] = Kalman gain matrix
 * Shat[L*L] = Covariance matrix
 * Csz[L*L] = Cross Covaraince matrix
 * L = Dimension of the vectors
 */
static void ukf_create_kalman_K(float *K, float *Shat, float *Csz, uint8_t L) {
	float b[L];
	float x[L];
	for (uint8_t j = 0; j < L; j++) {
		for (uint8_t i = 0; i < L; i++)
			*(b + i) = *(Csz + i * L + j); // For every column
		linsolve_chol(Shat, x, b, L); // Cholesky decomposition because Shat is symmetric and positive definite
		for (uint8_t i = 0; i < L; i++)
			*(K + i * L + j) = *(x + i); // Add column to the kalman K matrix
	}
}

/*
 * State update
 * K[L*L] = Kalman gain matrix
 * Shat[L*L] = Covariance matrix
 * P[L*L] = Covariance matrix
 * xhat[L] = Estimated vector
 * zk[L] = Output measurement vector
 * zhat[L] = Estimated vector
 * L = Dimension of the vectors
 */
static void ukf_state_update(float *K, float *Shat, float *P, float *xhat, float *zk, float *zhat, uint8_t L) {
	// K_zdiff = K*(zk - zhat)
	float zdiff[L];
	for (uint8_t i = 0; i < L; i++)
		*(zdiff + i) = *(zk + i) - *(zhat + i);
	float K_zdiff[L];
	mul(K, zdiff, K_zdiff, L, L, 1);

	// Final state update: xhat = xhat + K_zdiff
	for (uint8_t i = 0; i < L; i++)
		*(xhat + i) = *(xhat + i) + *(K_zdiff + i);

	// Solve P = P - K*Shat*K'
	// Transpose of K -> K'
	float KT[L * L];
	memcpy(KT, K, L * L * sizeof(float));
	tran(KT, L, L);

	// Multiply Shat times KT
	float ShatKT[L * L];
	mul(Shat, KT, ShatKT, L, L, L);

	// Multiply K times PyKT, we borrow matrix KT but we call it KShatKT
	mul(K, ShatKT, KT, L, L, L);

	// Update P = P - KShatKT
	for (uint8_t i = 0; i < L * L; i++)
		*(P + i) -= *(KT + i); // KT == KShatKT
}

/*
 * Create A = L*L^T
 * A need to be symmetric
 * A [m*n]
 * L [m*n]
 * n == m
 */
static void chol(float *A, float *L, int row) {
	memset(L, 0, row*row*sizeof(float));
	for (int i = 0; i < row; i++)
		for (int j = 0; j < (i + 1); j++) {
			float s = 0;
			for (int k = 0; k < j; k++)
				s += *(L + row * i + k) * *(L + row * j + k);

			// We cannot divide with zero
			if (*(L + row * j + j) == 0) {
				*(L + row * j + j) = FLT_EPSILON; // Same as eps command in MATLAB
			}
			*(L + row * i + j) = (i == j) ? sqrtf(*(A + row * i + i) - s) : (1.0 / *(L + row * j + j) * (*(A + row * i + j) - s));
		}
}

/*
 * C = A*B
 * A [row_a*column_a]
 * B [column_a*column_b]
 * C [row_a*column_b]
 */
static void mul(float* A, float* B, float* C, int row_a, int column_a, int column_b) {

	// Data matrix
	float* data_a;
	float* data_b;

	for (int i = 0; i < row_a; i++) {
		// Then we go through every column of b
		for (int j = 0; j < column_b; j++) {
			data_a = &A[i * column_a];
			data_b = &B[j];

			*C = 0; // Reset
			// And we multiply rows from a with columns of b
			for (int k = 0; k < column_a; k++) {
				*C += *data_a * *data_b;
				data_a++;
				data_b += column_b;
			}
			C++; // ;)
		}
	}
}
Rick81
Inlägg: 746
Blev medlem: 30 december 2005, 13:07:09

Re: Implementera ett UKF filter på inbyggda system?

Inlägg av Rick81 »

Glidande medelvärde kräver en kodrad så det är dom sagt stor skillnad på komplexiteten.

Frågan var ju om det var värt jobbet, och eftersom du inte har några krav så blir ju svaret att det inte är värt besväret...

Ger C-koden samma resultat som matlab?
DanielM
Inlägg: 2312
Blev medlem: 5 september 2019, 14:19:58

Re: Implementera ett UKF filter på inbyggda system?

Inlägg av DanielM »

Glidande medelvärde kräver dessutom t.ex. 50 punkter. UKF kräver bara 1 mätvärde.

Jag tycker det var värt jobbet.

C-koden ger exakt samma resultat som MATLAB-koden.
Jag har noggrant simulerat och bekräftat detta.

Förlåt för att jag skriver arrayer som *(array + index). Jag har för mig att pekare är snabbare än arrayer för inbyggda system. Det verkar genereras med assemblerkod för arrayer jämfört med pekare, iallafall i C#.
https://stackoverflow.com/questions/342 ... erformance
sodjan
EF Sponsor
Inlägg: 43231
Blev medlem: 10 maj 2005, 16:29:20
Ort: Söderköping

Re: Implementera ett UKF filter på inbyggda system?

Inlägg av sodjan »

Problemet är att du själv måste hålla reda på storleken på varje "entry" i arrayen istället för att bara stega 1, 2, 3 osv. Men jag vet inte, din jämförelse mellan pekare och arrayer är mest snömos och förvirrat. Jag har inte läst stacköverflow länken, men din tolkning låter underlig.
DanielM
Inlägg: 2312
Blev medlem: 5 september 2019, 14:19:58

Re: Implementera ett UKF filter på inbyggda system?

Inlägg av DanielM »

Jag har hört att det finns lite skillnader mellan pekare och arrayer.
Jag kan inte bara "stega" då jag använder arrayen flera gånger.

Jag har planerat att skriva om *(A + index) till A[index] om det skulle resultera samma assemblerkod.
Orsaken varför jag började *(A + index) har med att många av mina gamla bibliotek är skrivna så också. Nu talar jag om C kod som är skriven på säkert 80-talet. C89 snarare. Då gjorde jag likadant.
Men C89 är en föråldrad C-standard. Senaste är C18.
Rick81
Inlägg: 746
Blev medlem: 30 december 2005, 13:07:09

Re: Implementera ett UKF filter på inbyggda system?

Inlägg av Rick81 »

I C blir *(A + index) och A[index] samma sak i assembler. Jag tycker det är snyggare med A[index] för då ser man direkt i koden att man jobbar i arrayer.

Jag tror det du tänker på är att använda *A och A++, då bör det bli snabbare om du accesar *A flera ggr. Sen kan ju kompilatorns optimering kanske hitta det ändå. Enda sättet veta är att kolla assmeblern eller mäta hur lång tid de olika konstruktionerna tar.

C# är däremot helt annorlunda och där vet jag inte hur det blir.
sodjan
EF Sponsor
Inlägg: 43231
Blev medlem: 10 maj 2005, 16:29:20
Ort: Söderköping

Re: Implementera ett UKF filter på inbyggda system?

Inlägg av sodjan »

Bör det inte vara *(A + index*size), där size är storleken av varje entry, om nu inte varje entry är en enda byte.. .
DanielM
Inlägg: 2312
Blev medlem: 5 september 2019, 14:19:58

Re: Implementera ett UKF filter på inbyggda system?

Inlägg av DanielM »

Nej. Det behöver inte det som tur.
Jag ändrar om till [ ]
Mr Andersson
Inlägg: 1404
Blev medlem: 29 januari 2011, 21:06:30
Ort: Lapplandet

Re: Implementera ett UKF filter på inbyggda system?

Inlägg av Mr Andersson »

sodjan skrev: 27 juni 2021, 17:24:24 Bör det inte vara *(A + index*size), där size är storleken av varje entry, om nu inte varje entry är en enda byte.. .
Addition och subtraktion av pekare är lite speciella i C. Standarden säger att man ska hoppa n antal objekt och inte bytes.
Då kompilatorn vet storleken på varje typ så sker exakt det du skrev automatiskt under huven, men endast för pekare.
Om du konverterar din minnesadress från pekartyp till numerisk typ först så är det normal addition som gäller.

DanielM, Läser man svaren på SO så ser man ganska snart det här a[​i] is defined to be identical to *(a + i) in Standard
DanielM
Inlägg: 2312
Blev medlem: 5 september 2019, 14:19:58

Re: Implementera ett UKF filter på inbyggda system?

Inlägg av DanielM »

Mr Andersson skrev: 27 juni 2021, 18:13:54 DanielM, Läser man svaren på SO så ser man ganska snart det här a[​i] is defined to be identical to *(a + i) in Standard
Japp! Därför skriver jag om till [] istället. Ser snyggare ut, om man verkligen använder en array med andra ord :)

Men övrigt då?
Vad tycker ni om kalman filtret? Det är ju verkligen ett enkelt filter. :tumupp:
Skriv svar