| 1843 | | /*************************************/ |
| 1844 | | /* LQ_EKF_Inj */ |
| 1845 | | /*************************************/ |
| 1846 | | |
| 1847 | | /********************************************************************************/ |
| 1848 | | /* !!! potrebuje nutne rizeni z cidla, protoze si EKF dela sam */ |
| 1849 | | /********************************************************************************/ |
| 1850 | | |
| 1851 | | class PMSM_LQCtrl_EKF_Inj: public PMSMCtrl{ |
| 1852 | | public: |
| 1853 | | /* |
| 1854 | | PMSMCtrl: |
| 1855 | | double isa; |
| 1856 | | double isb; |
| 1857 | | double ome; |
| 1858 | | double the; |
| 1859 | | double Ww; |
| 1860 | | */ |
| 1861 | | |
| 1862 | | //PMSM parameters |
| 1863 | | const double a; |
| 1864 | | const double b; |
| 1865 | | const double c; |
| 1866 | | const double d; |
| 1867 | | const double e; |
| 1868 | | |
| 1869 | | const double Rs; |
| 1870 | | const double Lq; |
| 1871 | | const double Ld; |
| 1872 | | |
| 1873 | | //input penalty |
| 1874 | | double r; |
| 1875 | | double rpd; //penalize differences u - u_old |
| 1876 | | |
| 1877 | | //time step |
| 1878 | | const double Dt; |
| 1879 | | |
| 1880 | | //receding horizon |
| 1881 | | int rec_hor; |
| 1882 | | |
| 1883 | | //system matrices |
| 1884 | | mat A; //5x5 |
| 1885 | | mat B; //5x2 |
| 1886 | | //mat C; //2x5 |
| 1887 | | mat Q; //5x5 |
| 1888 | | mat R; //2x2 |
| 1889 | | mat Rpd; //2x4 |
| 1890 | | |
| 1891 | | //control matrix |
| 1892 | | mat L; |
| 1893 | | vec uab,udq; |
| 1894 | | vec icondpd; |
| 1895 | | vec u_old; |
| 1896 | | |
| 1897 | | //control maximum |
| 1898 | | double MAXu; |
| 1899 | | int MAXuflag; |
| 1900 | | |
| 1901 | | //lqg controler class |
| 1902 | | LQG_universal lq; |
| 1903 | | |
| 1904 | | //EKF input |
| 1905 | | vec y; |
| 1906 | | double isaold, isbold, omeold; |
| 1907 | | mat Akf; |
| 1908 | | mat Ckf; |
| 1909 | | mat Qkf; |
| 1910 | | mat Rkf; |
| 1911 | | vec uoldab; |
| 1912 | | mat Pkf; |
| 1913 | | const double R0,R10; |
| 1914 | | |
| 1915 | | //Inj |
| 1916 | | double iqold, iqnew; |
| 1917 | | double theold; |
| 1918 | | double obs, obsf, obsfold; |
| 1919 | | unsigned int timesync; |
| 1920 | | double koef; |
| 1921 | | double RC, alp; |
| 1922 | | |
| 1923 | | const double Vamp; //V |
| 1924 | | const double freq; //Hz |
| 1925 | | |
| 1926 | | // |
| 1927 | | // vec p_isd, p_isq, p_ome, p_the; |
| 1928 | | double p_isd, p_isq; |
| 1929 | | |
| 1930 | | PMSM_LQCtrl_EKF_Inj():PMSMCtrl(), a(0.9898), b(0.0072), c(0.0361), d(1.0), e(0.0149), Rs(0.28), Lq(0.003465), Ld(1.5*0.003465), |
| 1931 | | r(0.005), rpd(0.1), Dt(0.000125), rec_hor(10), //for r is a default value rewrited by pcp.txt file value |
| 1932 | | A(5, 5), B(5, 2), Q(5, 5), R(2, 2), Rpd(2, 4), |
| 1933 | | uab(2), udq(2), u_old(2), icondpd(8), MAXu(100.0), MAXuflag(0), Vamp(10), |
| 1934 | | Akf(4,4), Ckf(/*3*/2,4), Rkf(/*3,3*/2,2), Qkf(4,4), uoldab(2), Pkf(4,4), freq(500), |
| 1935 | | R0(0.003), R10(3), y(3) { |
| 1936 | | |
| 1937 | | timesync = 0; |
| 1938 | | iqnew = 0.0; |
| 1939 | | theold = 0.0; |
| 1940 | | isaold = 0.0; |
| 1941 | | isbold = 0.0; |
| 1942 | | omeold = 0.0; |
| 1943 | | |
| 1944 | | //set fix matrices elements & dimensions |
| 1945 | | A.zeros(); |
| 1946 | | A(0, 0) = A(1, 1) = a; |
| 1947 | | A(2, 1) = e; |
| 1948 | | A(2, 2) = d; |
| 1949 | | A(2, 4) = d - 1; |
| 1950 | | A(3, 2) = A(3, 4) = Dt; |
| 1951 | | A(3, 3) = A(4, 4) = 1.0; |
| 1952 | | B.zeros(); |
| 1953 | | B(0, 0) = B(1, 1) = c; |
| 1954 | | Q.zeros(); |
| 1955 | | Q(2, 2) = 1.0; |
| 1956 | | R.zeros(); |
| 1957 | | Rpd.zeros(); |
| 1958 | | u_old.zeros(); |
| 1959 | | uoldab.zeros(); |
| 1960 | | |
| 1961 | | koef = Vamp*(Ld-Lq)/(2*pi*freq*2*Ld*Lq); |
| 1962 | | RC = 1/(2*pi*5); //48LP |
| 1963 | | alp = Dt/(RC + Dt); |
| 1964 | | |
| 1965 | | Akf.zeros(); |
| 1966 | | Akf(0,0) = a; |
| 1967 | | Akf(1,1) = a; |
| 1968 | | Akf(2,2) = d; |
| 1969 | | Akf(3,3) = 1.0; |
| 1970 | | Akf(3,2) = Dt; |
| 1971 | | Ckf.zeros(); |
| 1972 | | Ckf(0,0) = 1.0; |
| 1973 | | Ckf(1,1) = 1.0; |
| 1974 | | //Ckf(2,2) = 0.0; |
| 1975 | | Qkf.zeros(); |
| 1976 | | Qkf(0,0) = 0.1;//0.0013; |
| 1977 | | Qkf(1,1) = 0.1;//0.0013; |
| 1978 | | Qkf(2,2) = 0.1;//5e-6; |
| 1979 | | Qkf(3,3) = 0.01;//1e-10; |
| 1980 | | Rkf.zeros(); |
| 1981 | | Rkf(0,0) = 0.05;//0.0006; |
| 1982 | | Rkf(1,1) = 0.05;//0.0006; |
| 1983 | | //Rkf(2,2) = 0.05;//0006; |
| 1984 | | //Pkf.zeros(); |
| 1985 | | Pkf = Qkf; |
| 1986 | | } |
| 1987 | | |
| 1988 | | double hfsignal(int time){ |
| 1989 | | return Vamp*cos(freq*2*pi*time*Dt); |
| 1990 | | } |
| 1991 | | |
| 1992 | | |
| 1993 | | virtual vec ctrlaction(const itpp::vec& cond) { |
| 1994 | | PMSMCtrl::ctrlaction(cond); // fills isa,isb,ome,the,Ww |
| 1995 | | |
| 1996 | | //increase synchronisation timer |
| 1997 | | timesync++; |
| 1998 | | |
| 1999 | | //get data from PMSM |
| 2000 | | //y(0) = isa;// + noise |
| 2001 | | //y(1) = isb;// + noise |
| 2002 | | //ome = 0; |
| 2003 | | //the = 0; |
| 2004 | | |
| 2005 | | //evaluate injection |
| 2006 | | /*iqold = iqnew; |
| 2007 | | iqnew = y(1)*cos(theold) - y(0)*sin(theold); |
| 2008 | | |
| 2009 | | obs = -(iqnew-(1 - Dt*Rs/Lq)*iqold)*hfsignal(timesync)/koef;//multiplication by HF signal |
| 2010 | | obsf = alp*obs + (1-alp)*obsfold; //lowpass filter |
| 2011 | | obsfold = obsf; |
| 2012 | | |
| 2013 | | double y2 = 2*(obsf);*/ |
| 2014 | | double isah, isbh, omeh, theh; |
| 2015 | | //EKF => isa=isaold,isb=isbold,ome=omeold,the=theold |
| 2016 | | isah = a*isaold + b*omeold*sin(theold) + c*uoldab(0); |
| 2017 | | isbh = a*isbold - b*omeold*cos(theold) + c*uoldab(1); |
| 2018 | | omeh = d*omeold + e*(isbold*cos(theold) - isaold*sin(theold)); |
| 2019 | | theh = theold + Dt*omeold; |
| 2020 | | |
| 2021 | | Akf(0, 2) = b*sin(theold); |
| 2022 | | Akf(0, 3) = b*omeold*cos(theold); |
| 2023 | | Akf(1, 2) = -b*cos(theold); |
| 2024 | | Akf(1, 3) = b*omeold*sin(theold); |
| 2025 | | Akf(2, 0) = -e*sin(theold); |
| 2026 | | Akf(2, 1) = e*cos(theold); |
| 2027 | | Akf(2, 3) = -e*(isaold*cos(theold)+isbold*sin(theold)); |
| 2028 | | |
| 2029 | | vec yiab(/*3*/2); |
| 2030 | | yiab(0) = /*y(0)*/isa - isah; |
| 2031 | | yiab(1) = /*y(1)*/isb - isbh; //ybe = sqrt(3.0)/3.0 * (2.0*ybe + yal); |
| 2032 | | //yiab(2) = y2 - theh; |
| 2033 | | |
| 2034 | | // Rkf(2,2) = R0 + (R10 - R0)*omeold/10; |
| 2035 | | Pkf = Akf*Pkf*Akf.T() + Qkf; |
| 2036 | | mat EKK = Pkf*Ckf.T()*inv(Ckf*Pkf*Ckf.T() + Rkf); |
| 2037 | | Pkf = Pkf - EKK*Ckf*Pkf; |
| 2038 | | //mat EKK = Pkf*Ckf.transpose()*inv(Ckf*Pkf*Ckf.transpose()+Rkf); |
| 2039 | | //Pkf = Akf*(Pkf - Pkf*Ckf.transpose()*inv(Ckf*Pkf*Ckf.transpose()+Rkf)*Ckf*Pkf)*Akf.transpose() + Qkf; |
| 2040 | | |
| 2041 | | // EKK = -P*C'*inv(C*P*C' + R)*yiab'; |
| 2042 | | // P = A*(P - P*C'*inv(C*P*C' + R)*C*P)*A' + Q; |
| 2043 | | |
| 2044 | | mat EKKy = EKK*yiab; |
| 2045 | | isah = isah + EKKy(0,0); |
| 2046 | | isbh = isbh + EKKy(1,0); |
| 2047 | | omeh = omeh + EKKy(2,0); |
| 2048 | | theh = theh + EKKy(3,0); |
| 2049 | | |
| 2050 | | //cout << omeh << endl; |
| 2051 | | |
| 2052 | | /*cout << "K" << isah << " " << isbh << " " << omeh << " " << theh << endl; |
| 2053 | | |
| 2054 | | isah = isa; |
| 2055 | | isbh = isb; |
| 2056 | | omeh = ome; |
| 2057 | | theh = the; |
| 2058 | | |
| 2059 | | cout << "M" << isah << " " << isbh << " " << omeh << " " << theh << endl;*/ |
| 2060 | | |
| 2061 | | isaold = isah; |
| 2062 | | isbold = isbh; |
| 2063 | | omeold = omeh; |
| 2064 | | theold = theh; |
| 2065 | | |
| 2066 | | int i; |
| 2067 | | vec tmp; |
| 2068 | | |
| 2069 | | lq.resetTime(); |
| 2070 | | |
| 2071 | | p_isd = isah*cos(theh) + isbh*sin(theh);//isa; |
| 2072 | | p_isq = isbh*cos(theh) - isah*sin(theh);//isb; |
| 2073 | | |
| 2074 | | A(0, 1) = Dt*omeh; |
| 2075 | | //A(0, 2) = Dt*p_isq; |
| 2076 | | //A(0, 4) = Dt*p_isq; |
| 2077 | | A(1, 0) = -Dt*omeh; |
| 2078 | | A(1, 2) = /*-Dt*p_isd*/-b; |
| 2079 | | A(1, 4) = /*-Dt*p_isd*/-b; |
| 2080 | | |
| 2081 | | lq.Models(0).A = A; |
| 2082 | | |
| 2083 | | //create control matrix |
| 2084 | | for(i = rec_hor; i > 0; i--){ |
| 2085 | | lq.redesign(); |
| 2086 | | } |
| 2087 | | lq.redesign(); |
| 2088 | | |
| 2089 | | L = lq.getL(); |
| 2090 | | |
| 2091 | | icondpd(0) = isah*cos(theh) + isbh*sin(theh); |
| 2092 | | icondpd(1) = isbh*cos(theh) - isah*sin(theh); |
| 2093 | | icondpd(2) = omeh - /*2.65*/Ww; |
| 2094 | | icondpd(3) = theh; |
| 2095 | | icondpd(4) = /*2.65*/Ww; |
| 2096 | | icondpd(5) = u_old(0); |
| 2097 | | icondpd(6) = u_old(1); |
| 2098 | | icondpd(7) = 0; |
| 2099 | | |
| 2100 | | tmp = L*icondpd; |
| 2101 | | |
| 2102 | | udq = tmp(0,1); |
| 2103 | | |
| 2104 | | //uab = udq; //set size |
| 2105 | | uab(0) = udq(0)*cos(theh) - udq(1)*sin(theh);// + hfsignal(timesync-1)*cos(theh); |
| 2106 | | uab(1) = udq(1)*cos(theh) + udq(0)*sin(theh);// + hfsignal(timesync-1)*sin(theh); |
| 2107 | | |
| 2108 | | |
| 2109 | | if(MAXuflag == 1){ //square cut off |
| 2110 | | if(uab(0) > MAXu) uab(0) = MAXu; |
| 2111 | | else if(uab(0) < -MAXu) uab(0) = -MAXu; |
| 2112 | | if(uab(1) > MAXu) uab(1) = MAXu; |
| 2113 | | else if(uab(1) < -MAXu) uab(1) = -MAXu; |
| 2114 | | } |
| 2115 | | else if(MAXuflag == 2){ //circular cut off |
| 2116 | | double uampl = sqrt(uab(0)*uab(0)+uab(1)*uab(1)); |
| 2117 | | double uangl = atan2(uab(1), uab(0)); |
| 2118 | | if (uampl > MAXu) uampl = MAXu; |
| 2119 | | uab(0) = uampl*cos(uangl); |
| 2120 | | uab(1) = uampl*sin(uangl); |
| 2121 | | } |
| 2122 | | |
| 2123 | | u_old = udq; |
| 2124 | | uoldab = uab; |
| 2125 | | |
| 2126 | | return uab; |
| 2127 | | } |
| 2128 | | |
| 2129 | | void from_setting(const Setting &set){ |
| 2130 | | PMSMCtrl::from_setting(set); |
| 2131 | | UI::get(r,set, "r", UI::optional); |
| 2132 | | UI::get(rec_hor,set, "h", UI::optional); |
| 2133 | | UI::get(MAXu,set, "MAXu", UI::optional); |
| 2134 | | UI::get(MAXuflag,set, "MAXuflag", UI::optional); |
| 2135 | | UI::get(rpd,set, "rpd", UI::optional); |
| 2136 | | } |
| 2137 | | |
| 2138 | | void validate(){ |
| 2139 | | R(0, 0) = R(1, 1) = r; |
| 2140 | | Rpd(0, 0) = Rpd(1, 1) = rpd; |
| 2141 | | Rpd(0, 2) = Rpd(1, 3) = -rpd; |
| 2142 | | |
| 2143 | | Array<quadraticfn> qloss(3); |
| 2144 | | qloss(0).Q.setCh(Q); |
| 2145 | | qloss(0).rv = RV("x", 5, 1); |
| 2146 | | qloss(1).Q.setCh(R); |
| 2147 | | qloss(1).rv = RV("u", 2, 0); |
| 2148 | | qloss(2).Q.setCh(Rpd); |
| 2149 | | qloss(2).rv = RV("u", 2, 0); |
| 2150 | | qloss(2).rv.add(RV("u", 2, -1)); |
| 2151 | | lq.Losses = qloss; |
| 2152 | | |
| 2153 | | //set lq |
| 2154 | | lq.rv = RV("u", 2, 0); |
| 2155 | | RV tmp = RV("x", 5, 0); |
| 2156 | | tmp.add(RV("u", 2, -1)); |
| 2157 | | lq.set_rvc(tmp); |
| 2158 | | |
| 2159 | | lq.horizon = rec_hor; |
| 2160 | | |
| 2161 | | Array<linfnEx> model(2); |
| 2162 | | model(0).A = A; |
| 2163 | | model(0).B = vec("0 0 0 0 0"); |
| 2164 | | model(0).rv = RV("x", 5, 0); |
| 2165 | | model(0).rv_ret = RV("x", 5, 1); |
| 2166 | | model(1).A = B; |
| 2167 | | model(1).B = vec("0 0"); |
| 2168 | | model(1).rv = RV("u", 2, 0); |
| 2169 | | model(1).rv_ret = RV("x", 5, 1); |
| 2170 | | lq.Models = model; |
| 2171 | | |
| 2172 | | lq.finalLoss.Q.setCh(Q); |
| 2173 | | lq.finalLoss.rv = RV("x", 5, 1); |
| 2174 | | |
| 2175 | | lq.validate(); |
| 2176 | | |
| 2177 | | uab.zeros(); |
| 2178 | | udq.zeros(); |
| 2179 | | } |
| 2180 | | }; |
| 2181 | | UIREGISTER(PMSM_LQCtrl_EKF_Inj); |
