Want Matlab code for Kalman Constraints

Hi Matlab Users, I got this Code, Whether is it correct or Not? If it is correct means give me the input values for 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

Answer 1

[Geoff Hayes]

Pearleesh - presumably you found this code from http://academic.csuohio.edu/simond/ConstrKF which the author (Dan Simon) used to generate results for his paper “Kalman Filtering with State Constraints: A Survey of Linear and Nonlinear Algorithms,” IET Control Theory & Applications, vol. 4, no. 8, pp. 1303-1318, August 2010. From the already noted link, download the complete set of program files that can be used to run the above function. See SystemConstr.m in particular.

[Pearleesh B]

Yes, Geoff Hayes 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]

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.