You are now following this question
- You will see updates in your followed content feed.
- You may receive emails, depending on your communication preferences.
I want matlab code for Kalman Filtering for Bandwidth and Energy Constrained Wireless Sensor Networks
1 view (last 30 days)
Show older comments
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 Comments
Geoff Hayes
on 18 Aug 2014
Pearleesh - rather than demanding code for a particular problem, you should make an initial attempt at solving it on your own. If you encounter problems with the software you have written, or are unsure of particular MATLAB syntax then by all means, please post a question within this forum.
To expect an exact Code solution for this Problem is unrealistic.
Pearleesh B
on 18 Aug 2014
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
Geoff Hayes
on 18 Aug 2014
Pearleesh - the above is way too many lines of code to be pasted into a comment (or question) and should be attached. So please attach (to your original question or a new comment) the m file for this code by using the paperclip button.
As for the error 'Q' undefined, please include the line number at which this error occurred, and include the line of code that you ran from the Command Window (or other file) to invoke this function. As Q is an input parameter to this function, the error message suggests that you are trying to use Q before it has been defined. Is this the case?
Pearleesh B
on 19 Aug 2014
Geoff Hayes
on 19 Aug 2014
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.)
Answers (0)
See Also
Community Treasure Hunt
Find the treasures in MATLAB Central and discover how the community can help you!
Start Hunting!An Error Occurred
Unable to complete the action because of changes made to the page. Reload the page to see its updated state.
Select a Web Site
Choose a web site to get translated content where available and see local events and offers. Based on your location, we recommend that you select: .
You can also select a web site from the following list
How to Get Best Site Performance
Select the China site (in Chinese or English) for best site performance. Other MathWorks country sites are not optimized for visits from your location.
Americas
- América Latina (Español)
- Canada (English)
- United States (English)
Europe
- Belgium (English)
- Denmark (English)
- Deutschland (Deutsch)
- España (Español)
- Finland (English)
- France (Français)
- Ireland (English)
- Italia (Italiano)
- Luxembourg (English)
- Netherlands (English)
- Norway (English)
- Österreich (Deutsch)
- Portugal (English)
- Sweden (English)
- Switzerland
- United Kingdom (English)
Asia Pacific
- Australia (English)
- India (English)
- New Zealand (English)
- 中国
- 日本Japanese (日本語)
- 한국Korean (한국어)