Test.zcos (6.0 KB)
I am currently facing issue simulation file or Input parameters. Both file are in attachment.
// Kalman_Filter_IF.sci
// The interface file for the generic Kalman Filter block.
// This block is externally activated to take measurements data.
// Takes the continuous positions velocity or accelerations as input, adds some
// errors (noise, scale, nonlinearity, misalignment) and gives
// it as the output.
// Abbrev.
// IF: Inertial Frame, BF: Body Frame,
// Model input:
// 2x2 Real - State vector with position and velocity BF w.r.t. IF
// 1 Real - Covariance vector
// Model output:
// 2x2 Real - Single axis gyro measurement output w.r.t. BF
// 1x1 Real - Error flag
// 0: No error
// 1: Positive saturation error
// 2: Negative saturation error
// Model states:
// 1x1 Real - Random walk error
// Model parameters:
// 2x2 Real - State Vector measurement unit vector w.r.t. BF (Unit vector)
// 1x1 Real - Noise mean (bias) of the sensors
// 1x1 Real - Noise stddev of the sensors
// 1x1 Real - Scale error
// 2x1 Real - Misalignment error
// 1x1 Real - Random walk error
function [x, y, typ] = Kalman_Filter_IF(job, arg1, arg2)
funcprot(0);
x = [];
y = [];
typ = [];
getInputFromSim = 1;
initial_state = [0.1; 0.0]; // 2*1 matrix
initial_covariance = [1.0, 0.0; 0.0, 1.0]; // 2*2 matrix
select job
// Initialize with mask parameters if any
case "plot" then
standard_draw(arg1);
case "getinputs" then
[x, y, typ] = standard_inputs(arg1);
case "getoutputs" then
[x, y, typ] = standard_outputs(arg1);
case "getorigin" then
[x, y] = standard_origin(arg1);
case 'set' then
// Assignments here comes after assignments of the execution with the "define" flag
x = arg1; // This line is essential, otherwise gives "Invalid index." error.
graphics = arg1.graphics;
exprs = graphics.exprs;
model = arg1.model;
if getInputFromSim == 1 then
exec("macros/Kalman_Filter_Utils.sci",-1); // Not sure about this line in MATLAB, comment it if not necessary
title = "Set Parameter";
labels = ['Initial state [x_dot(velocity), x(position)] which is position in ECEF frame and velocity';
'Covariance which gives the uncertainty at position and velocity'];
// Changed 'Stiffness' to 'Covariance'
// In future we can add {'Accelerations', 'Orientations which is obation altitude control system'}
types = list('vec', [2,1], 'mat', [2,2]); // Changed 'vec' to 'mat' for Covariance
endLoop = %f;
// Create a dialog for getting user inputs
while ~endLoop do
// Initialize dialog for parameters and initial state
[ok, State, Covariance, exprs] = scicos_getvalue(title, labels, types, exprs);
errMsg = [];
// Check return set conditions of the initial state as per meetings
if ~ok then
// depends on what type of data we are having
if State>0 then // first condition of position and velocity
errMsg = ["The velocity is positive [m/s]"];
end
if condition2 then // second condition of velocity
errMsg = [];
end
if isempty(errMsg) then
disp("Setting parameters...");
[model.rpar, isError] = EncodeRpar_KF(model.rpar, State, Covariance);
graphics.exprs = exprs;
x.graphics = graphics;
x.model = model;
endLoop = %t;
// Check parameters
// You can add additional checks for input parameters here if needed
// Accept inputs and save them
else
messagebox(errMsg);
error(errMsg, 20000);
endLoop = %f;
end
else
warning('Kalman_Filter_IF: Set: Cancel!');
endLoop = %t;
end
end
else
model.state = [concatAB(initial_state,initial_covariance)];
model.rpar = initial_covariance; // Use the covariance matrix as a parameter
graphics.exprs = exprs;
x.graphics = graphics;
x.model = model;
break
message("Failed to update block io");
end
// Concatenate the covariance matrix to the state vector
// model.state = [State; Covariance(:)]; // Use the concatenation operator ":" for the matrix
// Define block properties
case 'define'
// Create object
model = scicos_model();
// Provide name and type
model.sim = list('Kalman_Filter_sim', 5); // what is case 4 and 5? https://help.scilab.org/scicos_model.html
// Define inputs and outputs
// One input with a variable-size "double" element
model.in = [2;2]; // first dimension of the first input whcih is row 2*1 and 2*2 hence it will be [2;2]
model.in2 = [1;2]; // the second dimension of I/P port is [1;2] from above
model.intyp = [1;1]; // all the vector and matrices are real value
model.out = [2;2]; // first dimension of the first output whcih is row 2*1 and 2*2 hence it will be [2;2]
model.out2 = [1;2]; // the second dimension of O/P port is [1;2] from above
model.outtyp = [1;1]; // all the vector and matrices are real value
//model.state = [initial_state,matrix(initial_covariance)]; // Use the matrix concatenation operator ":" to convert matrix to column vector model.state=[State;Covariance]
model.state = [concatAB(initial_state,initial_covariance)];
model.rpar = [initial_covariance(:)]; // Use the covariance matrix as a parameter
// Define block properties
model.blocktype = "c";
model.dep_ut = [%t,%f]; // what does %t and %f indicates? true and false
// Set block properties
exprs = ["[0.1; 0.0]";"1.0, 0.0; 0.0, 1.0"]; // How do we get this value?
x = standard_define([4 4], model, exprs);
x.graphics.style = ['blockWithLabel;displayedLabel=Kalman_Filter'];
// other cases other than set or define sections
end
endfunction
function block = Kalman_Filter_SIM(block, flag)
exec("macros/Kalman_Filter_Utils.sci", -1);
select flag
case -5 // Error
verboseMessage("Calling with flag Error(-5)")
case 0 // Continuous State Prediction (Propagation)
verboseMessage("Calling with flag Continuous State Prediction (Propagation)(0)")
[x_kf, P_kf] = reverseAndReshape(block.x, 2);
// Kalman filter prediction step
F = [0 1; -1 0]; // State transition matrix (system dynamics) oscillator
Q = diag([0.01, 0.01]); // Process noise covariance matrix
// Update the state and covariance predictions
dx_kf = F * x_kf; // it should be vector // F next non linear
dP_kf = F * P_kf * F' + Q; // it should be matrix
block.xd = concatAB(dx_kf, dP_kf); // use concatAB
case 1 // Discrete Measurement Update
verboseMessage("Calling with flag Discrete Measurement Update(1)")
// Kalman filter measurement update step
H = [1 0]; // Measurement matrix
R = 0.1; // Measurement noise covariance
[x_kf, P_kf] = reverseAndReshape(block.x, 2);
// Compute Kalman gain
K = P_kf * H' / (H * P_kf * H' + R); // matrix inversion use
// Update the state estimate
z = block.inptr(1); // Measurement input
x_kf = x_kf + K * (z - H * x_kf); //linear function next step nonlinear function
// Update the state covariance
P_kf = (1 - K * H) * P_kf;
block.x = concatAB(x_kf, P_kf); // from vector to original martix and vector
block.outptr(1) = x_kf;
block.outptr(2) = P_kf;
case 2 // Discrete State Update
verboseMessage("Calling with flag Discrete State Update(2)")
case 3 // OutputEventTiming
verboseMessage("Calling with flag OutputEventTiming(3)")
case 4 // Initialization
verboseMessage("Calling with flag Initialization(4)")
// Kalman filter initial state and covariance
initial_state = [0.0; 0.0]; // 2*1 matrix
initial_covariance = [1.0, 0.0; 0.0, 1.0]; // 2*2 matrix
//model.cpar = initial_covariance; // Use covariance matrix as a parameter
// Initialize the state vector and covariance matrix
block.x = [initial_state; initial_covariance(:)];
case 5 // Ending
verboseMessage("Calling with flag Ending(5)")
case 6 // Reinitialization
verboseMessage("Calling with flag Reinitialization(6)")
case 9 // Zero-Crossing
verboseMessage("Calling with flag Zero-Crossing(9)")
else // Unknown flag
verboseMessage("Calling with unknown flag(" + string(flag) + ")")
end // end of select
endfunction
Can you please provide me where and how to locate the error? Input port or type means model which will be inteference file but simulation progress means simulation file issue in predictin step.
