I want matlab code for Kalman Filtering for Bandwidth and Energy Constrained Wireless Sensor Networks

Question

Pearleesh B 2014-8-18

0
链接

此问题的直接链接

https://ww2.mathworks.cn/matlabcentral/answers/151291-i-want-matlab-code-for-kalman-filtering-for-bandwidth-and-energy-constrained-wireless-sensor-network

评论： Geoff Hayes 2014-8-19

Hi Matlab Users, I have a problem to compute Bandwidth and Energy Constrained of Wireless Sensor Networks from Distributed Finite-Horizon Fusion Kalman Filtering(DFKF), The multiple binary random variables with known statistical properties were introduced to model the mixed constraints of bandwidth and energy, an optimal recursive DFKF was derived in the linear minimum variance sense by using the optimal fusion algorithm weighted by matrices. Finally the MSEs (Mean Square Error) of the designed DFKF were bounded or convergent. Please Give exact Code solution for this Problem.

5 个评论
显示 3更早的评论隐藏 3更早的评论

Pearleesh B 2014-8-18

在 MATLAB Online 中打开

Hi Geoff, I m having this Code to constraints my problem, i have run this code with proper input but it shows error as 'Q' undefined. Pls solve this problem.

function [EstErrors, ConstrErrors] = ... KalmanConstr(tf, T, horizon, A, B, H, D, d, Q, R, x, P, DisplayFlag, CorrectQFlag)

% This m-file simulates various Kalman filters. % This routine is called by SystemConstr.m. % For details about the algorithms, see http://academic.csuohio.edu/simond/ConstrKF. % INPUTS: tf = final time % T = step size % horizon = moving horizon estimator (MHE) horizon size % A = state transition matrix % B = input matrix % H = measurement matrix % D = constraint matrix (Dx=d) % d = constraint vector % Q = process noise covariance % R = measurement noise covariance % x = initial state % P = initial state estimate covariance % DisplayFlag = flag indicating whether to display and plot results % CorrectQFlag = flag indicating whether or not to use correct Q and P0 for filters % OUTPUTS: EstErrors = Array of RMS estimation errors. This is an array containing results for: % (1) The unconstrained Kalman filter % (2) The perfect measurement filter % (3) The estimate projection filter (W=P^{-1}) % (4) The moving horizon estimator % (5) The system projection filter % (6) The pdf truncation filter % ConstrErrors = Array of RMS constraint errors.

n = size(Q, 1); % number of states Rsqrt = sqrt®; xhat = x; % Unconstrained Kalman filter xhat1 = x; % Kalman filter with perfect measurements xtilde = x; % Constrained Kalman filter (estimate projection, W=inv(P)) xhatSys = x; % Constrained Kalman filter (system projection) xhatMHE = x; % Constrained Kalman filter (moving horizon estimation) xTrunc = x; % Constrained Kalman filter (pdf truncation)

% Measurement matrix for perfect measurement filter H1 = [H; D]; % Measurement noise covariance for perfect measurement filter R1 = R; for i = 1 : length(d) R1(end+1, end+1) = 0; end

% Initial estimation error covariance for constrained Kalman filter (system projection) [u, s, v] = svd(D'); r = length(d); % number of constraints u2 = u(:, r+1:end); PND = u2 * u2'; Pc = PND * P * PND; % Process noise covariance for constrained Kalman filter (system projection). % Note that this is the real process noise covariance. Qc = PND * Q * PND;

[dQc, lambdaQc, dQcT] = svd(Qc); for i = 1 : size(lambdaQc, 1) if real(lambdaQc(i,i)) < 0 % This should never be true, but it may occur because of numerical issues. lambdaQc(i,i) = 0; end end ddT = dQc * dQc'; if (norm(eye(size(ddT)) - ddT) > 1e-8) disp('Error - dQc is not orthogonal.'); return; end if (norm(dQc*lambdaQc*dQc' - Qc) > 1e-8) disp('Error - SVD failed for Qc'); return; end

if CorrectQFlag Q = Qc; P = Pc; end

% Initial estimation error covariance for perfect measurement formulation P1 = P;

% moving horizon estimation initialization Parray(:, :, 1) = P; P1array(:, :, 1) = P1; xhatMHE0 = xhat; zMHE = [];

% Initialize arrays for saving data for plotting. xarray = x; xhatarray = xhat; Constrarray = D * xhat; xhat1array = xhat1; Constr1array = D * xhat1; xtildearray = xtilde; ConstrTildearray = D * xtilde; xhatSysarray = xhatSys; ConstrSysarray = D * xhatSys; xhatMHEarray = xhatMHE; ConstrMHEarray = D * xhatMHE; xTruncArray = xTrunc; ConstrTruncArray = D * xTrunc;

randn('state', sum(100*clock)); In = eye(n, n);

% Begin the simulation. for t = T : T : tf u = 0; % If this is changed from zero then MHE needs to be modified % Simulate the system. x = A * x + B * u + dQc * sqrt(lambdaQc) * randn(size(x)); z = H * x + Rsqrt * randn(size(H,1), 1); % Run the Kalman filter. P = A * P * A' + Q; headinghat = atan2(xhat(3), xhat(4)); B = [0; 0; T*sin(headinghat); T*cos(headinghat)]; xhat = A * xhat + B * u; K = P * H' * inv(H * P * H' + R); xhat = xhat + K * (z - H * xhat); P = (In - K * H) * P; % Find the constrained (estimate projection) Kalman filter estimates. xtilde = xhat - P * D' * inv(D*P*D') * D * xhat; % Run the constrained Kalman filter (system projection). Pc = A * Pc * A' + Qc; headinghatSys = atan2(xhatSys(3), xhatSys(4)); B = [0; 0; T*sin(headinghatSys); T*cos(headinghatSys)]; xhatSys = A * xhatSys + B * u; Kc = Pc * H' * inv(H * Pc * H' + R); xhatSys = xhatSys + Kc * (z - H * xhatSys); Pc = (In - Kc * H) * Pc; % Run the filter for the perfect measurement formulation. z1 = [z; d]; P1 = A * P1 * A' + Q; headinghat1 = atan2(xhat1(3), xhat1(4)); B1 = [0; 0; T*sin(headinghat1); T*cos(headinghat1)]; xhat1 = A * xhat1 + B1 * u; S = H1 * P1 * H1' + R1; if rank(S) < length(z1) S = S + 1e-8; end K1 = P1 * H1' * inv(S); xhat1 = xhat1 + K1 * (z1 - H1 * xhat1); P1 = (In - K1 * H1) * P1; % Moving horizon estimation for linear system zMHE = [zMHE z]; if size(zMHE, 2) > horizon zMHE = zMHE(:, 2:end); lngthxhatMHEarray = size(xhatMHEarray, 2); xhatMHE0 = xhatMHEarray(:, lngthxhatMHEarray-horizon+1); end PMHE0 = Parray(:, :, 1); lngthP = size(Parray, 3) + 1; Parray(:, :, lngthP) = P; if lngthP > horizon Parray = Parray(:, :, 2:end); end % PMHE0 = P1array(:, :, 1); % lngthP = size(P1array, 3) + 1; % P1array(:, :, lngthP) = P1; % if lngthP > horizon % P1array = P1array(:, :, 2:end); % end xhatMHE = MHE(PMHE0, xhatMHE0, A, H, Q, R, zMHE, D, d); % Constrained filtering via probability density function truncation PTrunc = P; xTrunc = xhat; for k = 1 : r [Utrunc, Wtrunc, Vtrunc] = svd(PTrunc); Ttrunc = Utrunc; TTT = Ttrunc * Ttrunc'; if (norm(eye(size(TTT)) - TTT) > 1e-8) disp('Error - Ttrunc is not orthogonal.'); return; end if (norm(Utrunc*Wtrunc*Utrunc' - PTrunc) > 1e-8) disp('Error - SVD failed for pdf trunction'); return; end % Compute the modified Gram-Schmidt transformation S * Amgs = [ Wmgs ; 0 ]. % Amgs is a given n x m matrix, and S is an orthogonal n x n matrix, and Wmgs is an m x m matrix. Amgs = sqrt(Wtrunc) * Ttrunc' * D(k,:)'; % n x 1, where n = number of states [Wmgs, S] = MGS(Amgs); S = S * sqrt(D(k,:) * PTrunc * D(k,:)') / Wmgs; cTrunc = (d(k) - D(k,:) * xTrunc) / sqrt(D(k,:) * PTrunc * D(k,:)'); dTrunc = (d(k) - D(k,:) * xTrunc) / sqrt(D(k,:) * PTrunc * D(k,:)'); % The next 3 lines are for inequality constraints. In our example, they % are commented out because our problem uses equality constraints. %alpha = sqrt(2/pi) / erf(dTrunc/sqrt(2)) - erf(cTrunc/sqrt(2)); %mu = alpha * (exp(-cTrunc^2/2) - exp(-dTrunc^2/2)); %sigma2 = alpha * (exp(-cTrunc^2/2) * (cTrunc - 2 * mu) - exp(-dTrunc^2/2) * (dTrunc - 2 * mu)) + mu^2 + 1;

        % The following two lines are used for equality constraints.
        % Since we have equality constraints, the mean of zTrunc = cTrunc = dTrunc,
        % and its variance is 0.
        mu = cTrunc;
        sigma2 = 0;
        zTrunc = zeros(size(xTrunc));
        zTrunc(1) = mu;
        CovZ = eye(length(zTrunc));
        CovZ(1,1) = sigma2;
        xTrunc = Ttrunc * sqrt(Wtrunc) * S' * zTrunc + xTrunc;
        PTrunc = Ttrunc * sqrt(Wtrunc) * S' * CovZ * S * sqrt(Wtrunc) * Ttrunc';
    end
    % Compute the constraint errors
    ConstrErr = D * xhat - d;
    ConstrTilde = D * xtilde - d;
    Constr1Err = D * xhat1 - d;
    ConstrMHEErr = D * xhatMHE - d;
    ConstrSysErr = D * xhatSys - d;
    ConstrTruncErr = D * xTrunc - d;
    % Save data in arrays
    xarray = [xarray x];
    xhatarray = [xhatarray xhat];
    xtildearray = [xtildearray xtilde];
    xhat1array = [xhat1array xhat1];
    xhatMHEarray = [xhatMHEarray xhatMHE];    
    xhatSysarray = [xhatSysarray xhatSys]; 
    xTruncArray = [xTruncArray xTrunc];
    Constrarray = [Constrarray ConstrErr]; 
    ConstrTildearray = [ConstrTildearray ConstrTilde];
    Constr1array = [Constr1array Constr1Err];
    ConstrMHEarray = [ConstrMHEarray ConstrMHEErr];
    ConstrSysarray = [ConstrSysarray ConstrSysErr];
    ConstrTruncArray = [ConstrTruncArray ConstrTruncErr];
end % end simulation loop
% Compute RMS estimation errors - note we are computing the RMS estimation errors
% only of the first 2 states.
EstError = xarray - xhatarray;
EstError = mean(EstError(1,:).^2 + EstError(2,:).^2);
EstError = sqrt(EstError);
EstError1 = xarray - xhat1array;
EstError1 = mean(EstError1(1,:).^2 + EstError1(2,:).^2);
EstError1 = sqrt(EstError1);
EstErrorTilde = xarray - xtildearray;
EstErrorTilde = mean(EstErrorTilde(1,:).^2 + EstErrorTilde(2,:).^2);
EstErrorTilde = sqrt(EstErrorTilde);
EstErrorMHE = xarray - xhatMHEarray;
EstErrorMHE = mean(EstErrorMHE(1,:).^2 + EstErrorMHE(2,:).^2);
EstErrorMHE = sqrt(EstErrorMHE);
EstErrorSys = xarray - xhatSysarray;
EstErrorSys = mean(EstErrorSys(1,:).^2 + EstErrorSys(2,:).^2);
EstErrorSys = sqrt(EstErrorSys);
EstErrorTrunc = xarray - xTruncArray;
EstErrorTrunc = mean(EstErrorTrunc(1,:).^2 + EstErrorTrunc(2,:).^2);
EstErrorTrunc = sqrt(EstErrorTrunc);
% Compute constraint errors
r = length(d); % number of constraints
Constr = 0; for i = 1 : r, Constr = Constr + Constrarray(i,:).^2; end
Constr = sqrt(mean(Constr));
Constr1 = 0; for i = 1 : r, Constr1 = Constr1 + Constr1array(i,:).^2; end
Constr1 = sqrt(mean(Constr1));
ConstrTilde = 0; for i = 1 : r, ConstrTilde = ConstrTilde + ConstrTildearray(i,:).^2; end
ConstrTilde = sqrt(mean(ConstrTilde));
ConstrMHE = 0; for i = 1 : r, ConstrMHE = ConstrMHE + ConstrMHEarray(i,:).^2; end
ConstrMHE = sqrt(mean(ConstrMHE));
ConstrSys = 0; for i = 1 : r, ConstrSys = ConstrSys + ConstrSysarray(i,:).^2; end
ConstrSys = sqrt(mean(ConstrSys));
ConstrTrunc = 0; for i = 1 : r, ConstrTrunc = ConstrTrunc + ConstrTruncArray(i,:).^2; end
ConstrTrunc = sqrt(mean(ConstrTrunc));
EstErrors = [EstError, EstError1, EstErrorTilde, EstErrorMHE, EstErrorSys, EstErrorTrunc];
ConstrErrors = [Constr, Constr1, ConstrTilde, ConstrMHE, ConstrSys, ConstrTrunc];
if DisplayFlag
    close all; t = 0 : T : tf;
    figure;
    plot(t, xarray(1, :), ':', t, xarray(2, :), '-');
    title('True Position');
    xlabel('seconds'); ylabel('meters'); legend('north', 'east');
    figure;
    plot(t, xarray(1, :) - xhatarray(1, :), ':', t, xarray(2, :) - xhatarray(2, :), '-');
    title('Position Estimation Error (Unconstrained)');
    xlabel('seconds'); ylabel('meters'); legend('north', 'east');
    figure;
    plot(t, xarray(1, :) - xhat1array(1, :), ':', t, xarray(2, :) - xhat1array(2, :), '-');
    title('Position Estimation Error (Perfect Measurements)');
    xlabel('seconds'); ylabel('meters'); legend('north', 'east');
    figure;
    plot(t, xarray(1, :) - xtildearray(1, :), ':', t, xarray(2, :) - xtildearray(2, :), '-');
    title('Position Estimation Error (Estimate Projection, W=inv(P))');
    xlabel('seconds'); ylabel('meters'); legend('north', 'east');
    figure;
    plot(t, xarray(1, :) - xhatMHEarray(1, :), ':', t, xarray(2, :) - xhatMHEarray(2, :), '-');
    title('Position Estimation Error (MHE)');
    xlabel('seconds'); ylabel('meters'); legend('north', 'east');
    figure;
    plot(t, xarray(1, :) - xhatSysarray(1, :), ':', t, xarray(2, :) - xhatSysarray(2, :), '-');
    title('Position Estimation Error (System Projection)');
    xlabel('seconds'); ylabel('meters'); legend('north', 'east');
    figure;
    plot(t, xarray(1, :) - xTruncArray(1, :), ':', t, xarray(2, :) - xTruncArray(2, :), '-');
    title('Position Estimation Error (pdf Truncation)');
    xlabel('seconds'); ylabel('meters'); legend('north', 'east');
end

Pearleesh B 2014-8-19

Yes, But I need this kalman filtering for the application of Bandwidth and Energy Constraints of Wireless Sensor Networks with the help of Fusion Center, Can you give me good Solution?

Geoff Hayes 2014-8-19

Pealeesh - it is your responsibility to make an attempt at solving this problem. If you have specific questions about your implementation with respect to MATLAB syntax or errors that have been generated in the code, then by all mean please post a question in the forum. But to ask for a good solution to a problem such as the Bandwidth and Energy Constraints of Wireless Sensor Networks with the help of Fusion Center is unrealistic. (And attaching someone else's code is not helpful either.)

请先登录，再进行评论。

请先登录，再回答此问题。