// Kalman_Filter_SIM.sci
// Simulation file for the Kalman Filter block.
// Implements the Kalman Filter algorithm based on the provided parameters.
function block = Kalman_Filter_SIM(block, flag)
// Internal function for verbose messaging
function verboseMessage(debugMessage)
disp("[Kalman_Filter_SIM] time = " + string(scicos_time()) + "] {" + block.label + "}: " + debugMessage);
endfunction
// Retrieve n, p, r from block.ipar
n = block.ipar(1); // Order of the system
p = block.ipar(2); // Number of inputs
r = block.ipar(3); // Number of outputs
select flag
case -5 // Error
verboseMessage("Calling with flag Error(-5)")
// Handle error flag if necessary
case 4 // Initialization
verboseMessage("Calling with flag Initialization(4)")
// **1. Parse rpar**
expected_rpar_length = n*n + p*n + r*n + r*p + n*n + r*r; // A, B, C, D, Q, R
current_rpar_length = size(block.rpar, "*");
if current_rpar_length >= expected_rpar_length then
// Extract matrices
A = matrix(block.rpar(1:n*n), n, n);
B = matrix(block.rpar(n*n + 1:n*n + p*n), n, p);
C = matrix(block.rpar(n*n + p*n + 1:n*n + p*n + r*n), r, n);
D = matrix(block.rpar(n*n + p*n + r*n + 1:n*n + p*n + r*n + r*p), r, p);
Q = matrix(block.rpar(n*n + p*n + r*n + r*p + 1:n*n + p*n + r*n + r*p + n*n), n, n);
R = matrix(block.rpar(n*n + p*n + r*n + r*p + n*n + 1:n*n + p*n + r*n + r*p + n*n + r*r), r, r);
disp("Kalman_Filter_SIM: Parsed rpar successfully.");
// Dimension checks for matrices
if size(A,1) <> n | size(A,2) <> n then
verboseMessage("Matrix A dimension mismatch. Expected: " + string(n) + "x" + string(n) + ". Using identity matrix as default.");
A = eye(n, n);
end
if size(B,1) <> n | size(B,2) <> p then
verboseMessage("Matrix B dimension mismatch. Expected: " + string(n) + "x" + string(p) + ". Using zeros matrix as default.");
B = zeros(n, p);
end
if size(C,1) <> r | size(C,2) <> n then
verboseMessage("Matrix C dimension mismatch. Expected: " + string(r) + "x" + string(n) + ". Using zeros matrix as default.");
C = zeros(r, n);
end
if size(D,1) <> r | size(D,2) <> p then
verboseMessage("Matrix D dimension mismatch. Expected: " + string(r) + "x" + string(p) + ". Using zeros matrix as default.");
D = zeros(r, p);
end
if size(Q,1) <> n | size(Q,2) <> n then
verboseMessage("Matrix Q dimension mismatch. Expected: " + string(n) + "x" + string(n) + ". Using identity matrix as default.");
Q = eye(n, n);
end
if size(R,1) <> r | size(R,2) <> r then
verboseMessage("Matrix R dimension mismatch. Expected: " + string(r) + "x" + string(r) + ". Using identity matrix as default.");
R = eye(r, r);
end
else
verboseMessage("Error: block.rpar does not have enough elements for A, B, C, D, Q, R matrices.");
// Initialize default matrices
A = eye(n, n);
B = zeros(n, p);
C = zeros(r, n);
D = zeros(r, p);
Q = 0.01 * eye(n, n);
R = 0.1 * eye(r, r);
block.rpar = [A(:); B(:); C(:); D(:); Q(:); R(:)];
disp("Kalman_Filter_SIM: Initialized A, B, C, D, Q, R with default values.");
end
// **2. Initialize State and Covariance**
expected_state_length = n + n*n; // x_k + P_k
if size(block.x, "*") >= expected_state_length then
x_k = matrix(block.x(1:n), n, 1);
P_k = matrix(block.x(n+1:$), n, n);
disp("Kalman_Filter_SIM: Initialized state x_k and covariance P_k.");
// Dimension checks for state
if size(x_k,1) <> n | size(x_k,2) <> 1 then
verboseMessage("State vector x_k dimension mismatch. Expected: " + string(n) + "x1. Using zeros vector as default.");
x_k = zeros(n, 1);
end
if size(P_k,1) <> n | size(P_k,2) <> n then
verboseMessage("Covariance matrix P_k dimension mismatch. Expected: " + string(n) + "x" + string(n) + ". Using identity matrix as default.");
P_k = eye(n, n);
end
else
verboseMessage("Error: block.x does not have enough elements. Assigning default values.");
x_k = zeros(n, 1);
P_k = eye(n, n);
block.x = [x_k; P_k(:)];
disp("Kalman_Filter_SIM: Assigned default state x_k and covariance P_k.");
end
case 0 // State Derivative Computation (Prediction and Update Steps)
verboseMessage("Calling with flag StateDerivativeComputation(0)")
// **1. Extract current state and covariance**
if size(block.x, "*") >= (n + n*n) then
x_k = matrix(block.x(1:n), n, 1);
P_k = matrix(block.x(n+1:$), n, n);
disp("Kalman_Filter_SIM: Current state x_k:");
disp(x_k);
disp("Kalman_Filter_SIM: Current covariance P_k:");
disp(P_k);
// Dimension checks for state
if size(x_k,1) <> n | size(x_k,2) <> 1 then
verboseMessage("State vector x_k dimension mismatch before prediction. Expected: " + string(n) + "x1. Using zeros vector as default.");
x_k = zeros(n, 1);
end
if size(P_k,1) <> n | size(P_k,2) <> n then
verboseMessage("Covariance matrix P_k dimension mismatch before prediction. Expected: " + string(n) + "x" + string(n) + ". Using identity matrix as default.");
P_k = eye(n, n);
end
else
verboseMessage("Error: block.x does not have enough elements. Initializing to default [0; ...; 0].");
x_k = zeros(n, 1);
P_k = eye(n, n);
block.x = [x_k; P_k(:)];
end
// **2. Extract Inputs**
// Measurement inputs y_meas (r x 1) and control inputs u_ctrl (p x 1)
// Assuming first input port is original signal (no error) and second is noisy signal (with error).
// If you want to separate measurement input from original and noisy signals, adjust logic accordingly.
// In this code structure:
// block.inptr(1) => Original signal
// block.inptr(2) => Noisy measurement signal
// block.inptr(3) => control input (if present)
// Extract Original Signal from first input port
if block.nin >=1 then
if size(block.inptr(1), "*") >= r then
original_signal = matrix(block.inptr(1), r, 1);
else
original_signal = zeros(r, 1);
verboseMessage("Error: Original signal input does not have enough elements. Setting original_signal to 0.");
end
else
original_signal = zeros(r, 1);
verboseMessage("Error: block.nin <1. Setting original_signal to 0.");
end
// Extract Noisy measurement input from second input port
if block.nin >=2 then
if size(block.inptr(2), "*") >= r then
y_meas = matrix(block.inptr(2), r, 1);
else
y_meas = zeros(r, 1);
verboseMessage("Error: Noisy measurement input does not have enough elements. Setting y_meas to 0.");
end
else
y_meas = zeros(r, 1);
verboseMessage("Error: block.nin <2. Setting y_meas to 0.");
end
// Extract Control Inputs u_ctrl from third input port (if p>0)
if block.nin >=3 & p > 0 then
if size(block.inptr(3), "*") >= p then
u_ctrl = matrix(block.inptr(3), p, 1);
else
u_ctrl = zeros(p, 1);
verboseMessage("Error: Control input does not have enough elements. Setting u_ctrl to 0.");
end
else
u_ctrl = zeros(p, 1);
if p > 0 then
verboseMessage("Error: block.nin <3. Setting u_ctrl to 0.");
end
end
disp("Kalman_Filter_SIM: Original signal:");
disp(original_signal);
disp("Kalman_Filter_SIM: Measurement y_meas:");
disp(y_meas);
disp("Kalman_Filter_SIM: Control input u_ctrl:");
disp(u_ctrl);
// **3. Check if All Matrices are Initialized Properly**
if isempty(A) | isempty(B) | isempty(C) | isempty(D) | isempty(Q) | isempty(R) then
verboseMessage("Error: One or more system matrices (A, B, C, D, Q, R) are empty.");
// Assign default matrices to prevent further errors
A = eye(n, n);
B = zeros(n, p);
C = zeros(r, n);
D = zeros(r, p);
Q = 0.01 * eye(n, n);
R = 0.1 * eye(r, r);
// Optionally, halt simulation
error("Kalman_Filter_SIM: Missing system matrices. Simulation halted.");
end
// Additional dimension checks specifically for the update step
if size(A,1) <> n | size(A,2) <> n then
verboseMessage("Matrix A dimension mismatch before prediction. Expected: " + string(n) + "x" + string(n) + ". Using identity matrix as default.");
A = eye(n, n); // Default fallback to an identity matrix
end
if size(B,1) <> n | size(B,2) <> p then
verboseMessage("Matrix B dimension mismatch before prediction. Expected: " + string(n) + "x" + string(p) + ". Using zeros matrix as default.");
B = zeros(n, p); // Default fallback to a zeros matrix
end
if size(x_k,1) <> n | size(x_k,2) <> 1 then
verboseMessage("State vector x_k dimension mismatch before prediction. Expected: " + string(n) + "x1. Using zeros vector as default.");
x_k = zeros(n, 1); // Default fallback to zeros vector
end
if size(u_ctrl,1) <> p | size(u_ctrl,2) <> 1 then
verboseMessage("Control input u_ctrl dimension mismatch before prediction. Expected: " + string(p) + "x1. Using zeros vector as default.");
u_ctrl = zeros(p, 1); // Default fallback to zeros vector
end
if size(P_k,1) <> n | size(P_k,2) <> n then
verboseMessage("Covariance matrix P_k dimension mismatch before update. Expected: " + string(n) + "x" + string(n) + ". Using identity matrix as default.");
P_k = eye(n, n); // Default fallback to identity matrix
end
if size(Q,1) <> n | size(Q,2) <> n then
verboseMessage("Process Noise Covariance Q dimension mismatch before update. Expected: " + string(n) + "x" + string(n) + ". Using identity matrix as default.");
Q = eye(n, n); // Default fallback to identity matrix
end
// **4. Prediction Step**
x_pred = A * x_k + B * u_ctrl;
P_pred = A * P_k * A' + Q;
disp("Kalman_Filter_SIM: Prediction x_pred:");
disp(x_pred);
disp("Kalman_Filter_SIM: Prediction P_pred:");
disp(P_pred);
// Additional dimension checks before computing S at line 209
if size(C,1) <> r | size(C,2) <> n then
verboseMessage("Matrix C dimension mismatch before measurement update. Expected: " + string(r) + "x" + string(n) + ". Using zeros matrix as default.");
C = zeros(r, n); // Default fallback to zeros matrix
end
if size(P_pred,1) <> n | size(P_pred,2) <> n then
verboseMessage("Predicted covariance matrix P_pred dimension mismatch before measurement update. Expected: " + string(n) + "x" + string(n) + ". Using identity matrix as default.");
P_pred = eye(n, n); // Default fallback to identity matrix
end
if size(R,1) <> r | size(R,2) <> r then
verboseMessage("Matrix R dimension mismatch before measurement update. Expected: " + string(r) + "x" + string(r) + ". Using identity matrix as default.");
R = eye(r, r); // Default fallback to identity matrix
end
// **5. Update Step**
// Compute Innovation Covariance S
S = C * P_pred * C' + R;
// Dimension check for S
if size(S,1) <> r | size(S,2) <> r then
verboseMessage("Innovation covariance matrix S dimension mismatch. Expected: " + string(r) + "x" + string(r) + ". Using identity matrix as default.");
S = eye(r, r); // Default fallback to identity matrix
end
if ~isempty(find(S == 0)) then
verboseMessage("Warning: Innovation covariance S has zero elements. Adjusting to avoid division by zero.");
S = S + 1e-6 * eye(r, r); // Add a small value to diagonal
end
K = P_pred * C' / S;
disp("Kalman_Filter_SIM: Kalman Gain K:");
disp(K);
// Compute innovation
y_pred = C * x_pred;
innovation = y_meas - y_pred;
disp("Kalman_Filter_SIM: Innovation:");
disp(innovation);
// Update state estimate and covariance
x_upd = x_pred + K * innovation;
P_upd = (eye(n) - K * C) * P_pred;
disp("Kalman_Filter_SIM: Updated state x_upd:");
disp(x_upd);
disp("Kalman_Filter_SIM: Updated covariance P_upd:");
disp(P_upd);
// **6. Assign Updated State to block.x**
if ~isempty(x_upd) & ~isempty(P_upd(:)) then
block.x = [x_upd; P_upd(:)];
disp("Kalman_Filter_SIM: block.x updated.");
else
verboseMessage("Error: x_upd or P_upd is empty. Cannot update block.x.");
end
case 1 // Output Computation
verboseMessage("Calling with flag OutputComputation(1)")
// **1. Extract current state**
if size(block.x, "*") >= n then
x_hat = matrix(block.x(1:n), n, 1);
disp("Kalman_Filter_SIM: Estimated state x_hat:");
disp(x_hat);
else
verboseMessage("Error: block.x does not have enough elements for state estimate.");
x_hat = zeros(n, 1);
end
// **2. Assign Outputs**
// Output 1: Original signal
// Output 2: Noisy signal
// Output 3: Kalman filter estimated state
if block.nin >= 1 then
block.outptr(1) = block.inptr(1); // Original signal
else
block.outptr(1) = zeros(r, 1);
end
if block.nin >= 2 then
block.outptr(2) = block.inptr(2); // Noisy signal
else
block.outptr(2) = zeros(r, 1);
end
block.outptr(3) = x_hat; // Estimated state
verboseMessage("Output assigned: Original signal, Noisy signal, and Kalman estimate.");
case 2 // Discrete State Update
verboseMessage("Calling with flag DiscreteStateUpdate(2)")
// Implement discrete state update logic here if needed
case 3 // Output Event Timing
verboseMessage("Calling with flag OutputEventTiming(3)")
// Implement output event timing logic here if needed
case 5 // Ending
verboseMessage("Calling with flag Ending(5)")
disp("End of simulation for Kalman Filter block.")
case 6 // Reinitialization
verboseMessage("Calling with flag Reinitialization(6)")
// Handle reinitialization if necessary
// Example: Reset state and covariance
if size(block.rpar, "*") >= (n*n + p*n + r*n + r*p + n*n + r*r) then
A = matrix(block.rpar(1:n*n), n, n);
B = matrix(block.rpar(n*n + 1:n*n + p*n), n, p);
C = matrix(block.rpar(n*n + p*n + 1:n*n + p*n + r*n), r, n);
D = matrix(block.rpar(n*n + p*n + r*n + 1:n*n + p*n + r*n + r*p), r, p);
Q = matrix(block.rpar(n*n + p*n + r*n + r*p + 1:n*n + p*n + r*n + r*p + n*n), n, n);
R = matrix(block.rpar(n*n + p*n + r*n + r*p + n*n + 1:n*n + p*n + r*n + r*p + n*n + r*r), r, r);
else
verboseMessage("Error: block.rpar does not have enough elements for Reinitialization.");
A = eye(n, n);
B = zeros(n, p);
C = zeros(r, n);
D = zeros(r, p);
Q = 0.01 * eye(n, n);
R = 0.1 * eye(r, r);
end
// Reset state and covariance
x0_vector = zeros(n, 1); // Default initial state (n x 1)
P0_matrix = eye(n, n); // Default initial covariance (n x n)
block.x = [x0_vector; P0_matrix(:)]; // Reset block.x with state and covariance
disp("Kalman_Filter_SIM: block.x reset to initial state and covariance.")
case 9 // Zero-Crossing
verboseMessage("Calling with flag Zero-Crossing(9)")
// Not used in this system
otherwise // Unknown flag
verboseMessage("Calling with unknown flag(" + string(flag) + ")")
disp("Unknown flag for Kalman_Filter_SIM")
end
endfunction
I have kalman filter block but the output of block is not correct. Can anyone tell me where is the error?
Only_kalman_filter_testing.zcos (3.7 KB)
Why kalman filter block line is giving me straight line? I think there is an error Kalman_Filter_SIM function