Library IEEE ; Use IEEE . std_logic_1164 .All ; Use IEEE .numeric_std .All ; Package matrix_types Is Type matrix_2x2 Is Array (1 to 2, 1 to 2) Of Signed (31 downto 0); --row x column Type matrix_2x1 Is Array (1 to 2) Of Signed (31 downto 0); --row x column Type matrix_1x2 Is Array (1 to 2) Of Signed (31 downto 0); --row x column End Package matrix_types ; Library IEEE ; Use IEEE . std_logic_1164 .All ; Use IEEE .numeric_std .All ; use IEEE.std_logic_signed.all; --use IEEE.std_logic_unsigned.all; use IEEE.std_logic_arith.all; Use work .matrix_types .All ; Use std. textio .All ; Entity kf_top Is Port ( z_position : In Signed (31 downto 0); reset :In Std_logic ; position , velocity : Out Signed (31 downto 0)); End kf_top ; Architecture kf_behav Of kf_top Is --signal K2: SIGNED(31 downto 0); -- ***************************************** --* Function to multiply two 2x2 matrices * -- ***************************************** Function matrix_mult_2x2 (A,B: matrix_2x2 ) Return matrix_2x2 Is Variable result : matrix_2x2 ; Variable func_temp1 : Signed (63 downto 0) :=( OTHERS => '0'); Begin --Begin function code . For i In 1 to 2 Loop For L In 1 to 2 Loop For j In 1 to 2 Loop func_temp1 := (A(i,j)*B(j,L)) + func_temp1 ; End Loop ; result (i,L) := func_temp1 (47 downto 16) ; func_temp1 :=( OTHERS => '0'); End Loop ; End Loop ; Return result ; End matrix_mult_2x2 ; -- ************************************ --* Function to add two 2x2 matrices * -- ************************************ Function matrix_add_2x2 (A,B: matrix_2x2 ) Return matrix_2x2 Is Variable result : matrix_2x2 ; Begin --Begin function code . For i In 1 to 2 Loop For j In 1 to 2 Loop result (i,j) := A(i,j)+B(i,j); End Loop ; End Loop ; Return result ; End matrix_add_2x2 ; -- ******************************************** --* Function to add a scalar to a 2x2 matrix * -- ******************************************** Function matrix_add_int_2x2 (A: matrix_2x2 ;B: Signed (31 downto 0)) Return matrix_2x2 Is Variable result : matrix_2x2 ; Begin --Begin function code . For i In 1 to 2 Loop For j In 1 to 2 Loop result (i,j) := A(i,j)+B; End Loop ; End Loop ; End matrix_add_int_2x2 ; -- ************************************************ --* Function to multiply a 2x2 with a 2x1 matrix * -- ************************************************ Function matrix_mult_2x2_2x1 (A: matrix_2x2 ;B: matrix_2x1 ) Return matrix_2x1 Is Variable result : matrix_2x1 ; Variable func_temp1 : Signed (63 downto 0) :=( OTHERS => '0'); Begin --Begin function code . For i In 1 to 2 Loop For j In 1 to 2 Loop func_temp1 := (A(i,j)*B(j)) + func_temp1 ; End Loop ; result (i) := func_temp1 (47 downto 16); func_temp1 := ( OTHERS=> '0'); End Loop ; Return result ; End matrix_mult_2x2_2x1 ; -- ************************************************* --* Function to multiply a 1x2 with a 2x2 matrix * -- ************************************************* Function matrix_mult_1x2_2x2 (A: matrix_1x2 ;B: matrix_2x2 ) Return matrix_1x2 Is Variable result : matrix_1x2 ; Variable func_temp1 : Signed (63 downto 0) := ( OTHERS=> '0'); Begin --Begin function code . For L In 1 to 2 Loop For j In 1 to 2 Loop func_temp1 := (A(j)*B(j,L)) + func_temp1 ; End Loop ; result (L) := func_temp1 (47 downto 16); func_temp1 :=( OTHERS => '0'); End Loop ; Return result ; End matrix_mult_1x2_2x2 ; -- ************************************************* --* Function to multiply a 1x2 with a 2x1 matrix * -- ************************************************* Function matrix_mult_1x2_2x1 (A: matrix_1x2 ;B: matrix_2x1 ) Return Signed Is Variable result : Signed (63 downto 0) :=( OTHERS => '0'); Begin --Begin function code . For j In 1 to 2 Loop result := (A(j)*B(j)) + result ; End Loop ; Return result (47 downto 16); End matrix_mult_1x2_2x1 ; -- ************************************************ --* Function to multiply a 2x1 and a 1x2 matrix * -- ************************************************ Function matrix_mult_2x1_1x2 (A: matrix_2x1 ;B: matrix_1x2 ) Return matrix_2x2 Is Variable result : matrix_2x2 ; Variable func_temp1 : Signed (63 downto 0); Begin --Begin function code . For i In 1 to 2 Loop For L In 1 to 2 Loop func_temp1 := (A(i)*B(L)); result (i,L) := func_temp1 (47 downto 16) ; End Loop ; end loop; Return result ; End matrix_mult_2x1_1x2 ; -- ************************************************** --* Function to multiply a 2x1 matrix and a scalar * -- ************************************************** Function matrix_mult_2x1_int (A: matrix_2x1 ;B: Signed (31 downto 0)) Return matrix_2x1 Is Variable result : matrix_2x1 ; Variable func_temp1 : Signed (63 downto 0); Begin --Begin function code . For i In 1 to 2 Loop func_temp1 := A(i)*B; result (i) := func_temp1 (47 downto 16); End Loop ; Return result ; End matrix_mult_2x1_int ; -- ***************************************** --* Function to add a 2x1 to a 2x1 matrix * -- ***************************************** Function matrix_add_2x1_2x1 (A: matrix_2x1 ;B: matrix_2x1 ) Return matrix_2x1 Is Variable result : matrix_2x1 ; Begin --Begin function code . For i In 1 to 2 Loop result (i) := A(i)+B(i); End Loop ; Return result ; End matrix_add_2x1_2x1 ; -- ***************************************** --* Function to subtract two 2x2 matrices * -- ***************************************** Function matrix_subtract_2x2 (A,B: matrix_2x2 ) Return matrix_2x2 Is Variable result : matrix_2x2 ; Begin --Begin function code . For i In 1 to 2 Loop For j In 1 to 2 Loop result (i,j) := A(i,j)-B(i,j); End Loop ; End Loop ; Return result ; End matrix_subtract_2x2 ; -- ********************************************************** --* Function to return the diagonal ( diag ) of a 2x2 matrix * -- ********************************************************** Function diag_2x2 (A: matrix_2x2 ) Return matrix_2x1 Is Variable result : matrix_2x1 ; Begin --Begin function code . For i In 1 to 2 Loop result (i) := A(i,i); End Loop ; Return result ; End diag_2x2 ; -- *********************** --* Begin main process . * -- *********************** Begin Process ( z_position , reset ) Is Constant dt : Signed (31 downto 0) :="00000000000000000001100110011001"; --R is the measurement noise covariance . Constant R : Signed (31 downto 0) :="00000000000010100000000000000000"; --Q is " dynamic noise strength " ( process noise covariance ). Constant Q : Signed (31 downto 0) :="00000000011001000000000000000000"; --G is the noise injection model . --This was intended to be 2 rows , 1 column but is represented as1 row , 2 columns . Constant G : matrix_2x1 := ("00000000000000000000000000000000", "00000000000000010000000000000000"); --This was intended to be 2 rows , 1 column but is represented as --1 row , 2 columns . Constant B : matrix_2x1 := ("00000000000000000000000000000000", "00000000000000010000000000000000"); --This was intended to be 2 rows , 1 column but is represented as --1 row , 2 columns . Constant Bd : matrix_2x1 := ("00000000000000000000000101000111", "00000000000000000001100110011001"); Constant H : matrix_1x2 := ("00000000000000010000000000000000", "00000000000000000000000000000000"); Constant H_prime : matrix_2x1 := ("00000000000000010000000000000000", "00000000000000000000000000000000"); Constant F : matrix_2x2 := (("00000000000000000000000000000000", "00000000000000010000000000000000"), ("00000000000000000000000000000000", "00000000000000000000000000000000")); Constant phi : matrix_2x2 := (("00000000000000010000000000000000", "00000000000000000001100110011001"), ("00000000000000000000000000000000", "00000000000000010000000000000000")); Constant phi_prime : matrix_2x2 := (("00000000000000010000000000000000", "00000000000000000000000000000000"), ("00000000000000000001100110011001", "00000000000000010000000000000000")); Constant Qd : matrix_2x2 := (("00000000000000000000100010000110", "00000000000000001000000000000000"), ("00000000000000001000000000000000", "00000000000010100000000000000000")); Constant Gd : matrix_2x2 := (("00000000000000010000000000000000", "00000000000000000000000000000000"), ("00000000000000000000000000000000", "00000000000000010000000000000000")); -- *************************** --* Definition of Variables * -- *************************** Variable x : matrix_2x1 := ("00000000000000010000000000000000", "00000000000000010000000000000000"); Variable P : matrix_2x2 := (("00000000000000000100000000000000", "00000000000000000000000000000000"), ("00000000000000000000000000000000", "00000000000000000100000000000000")); Variable A: signed (31 downto 0); Variable residual : Signed (31 downto 0); Variable K : matrix_2x1 ; Variable K_temp : signed (31 downto 0); Begin If reset = '1' Then x := ("00000000000000010000000000000000", "00000000000000010000000000000000"); P := (("00000000000000000100000000000000", "00000000000000000000000000000000"), ("00000000000000000000000000000000", "00000000000000000100000000000000")); End If; x := matrix_mult_2x2_2x1 (phi ,x); P := matrix_add_2x2 (( matrix_mult_2x2 (matrix_mult_2x2 (phi ,P),phi_prime )),Qd); A :=matrix_mult_1x2_2x1 ( matrix_mult_1x2_2x2 (H,P),H_prime )+R; --error is here-"Operator must have constant operands or first operand must be power of 2 K_temp := "0100000000000000000000000000000000" /A ; K := matrix_mult_2x1_int ( matrix_mult_2x2_2x1 (P,H_prime ),K_temp (31 downto 0)); residual := z_position - matrix_mult_1x2_2x1 (H,x); x := matrix_add_2x1_2x1 (x, matrix_mult_2x1_int (K,residual)); P := matrix_subtract_2x2 (P, matrix_mult_2x2 (matrix_mult_2x1_1x2 (K,H),P)); position<= x(1); velocity<= x(2); End Process ; End kf_behav;