| 178 | |
| 179 | template <long S, long I> |
| 180 | unsigned long solve_linear_mpc ( |
| 181 | const matrix<double,S,S>& A, |
| 182 | const matrix<double,S,I>& B, |
| 183 | const matrix<double,S,1>& C, |
| 184 | const matrix<double,S,1>& Q, |
| 185 | const matrix<double,I,1>& R, |
| 186 | const matrix<double,I,1>& _lower, |
| 187 | const matrix<double,I,1>& _upper, |
| 188 | const std::vector<matrix<double,S,1> >& target, |
| 189 | const matrix<double,S,1>& initial_state, |
| 190 | std::vector<matrix<double,I,1> >& controls // input and output |
| 191 | ) |
| 192 | { |
| 193 | using namespace impl_mpc; |
| 194 | DLIB_CASSERT(target.size() == controls.size(),""); |
| 195 | |
| 196 | matrix<double> K(B.nr()*controls.size(), B.nc()*controls.size()); |
| 197 | matrix<double,0,1> M(B.nr()*controls.size()); |
| 198 | |
| 199 | // compute powers of A: Apow[i] == A^i |
| 200 | std::vector<matrix<double,S,S> > Apow(controls.size()); |
| 201 | Apow[0] = identity_matrix(A); |
| 202 | for (unsigned long i = 1; i < Apow.size(); ++i) |
| 203 | Apow[i] = A*Apow[i-1]; |
| 204 | |
| 205 | // fill in K |
| 206 | K = 0; |
| 207 | for (unsigned long r = 0; r < controls.size(); ++r) |
| 208 | for (unsigned long c = 0; c <= r; ++c) |
| 209 | set_subm(K,r*B.nr(),c*B.nc(), B.nr(), B.nc()) = Apow[r-c]*B; |
| 210 | |
| 211 | // fill in M |
| 212 | set_subm(M,0*A.nr(),0,A.nr(),1) = A*initial_state + C; |
| 213 | for (unsigned long i = 1; i < controls.size(); ++i) |
| 214 | set_subm(M,i*A.nr(),0,A.nr(),1) = A*subm(M,(i-1)*A.nr(),0,A.nr(),1) + C; |
| 215 | |
| 216 | //cout << "M: \n" << M << endl; |
| 217 | //cout << "K: \n" << K << endl; |
| 218 | |
| 219 | matrix<double,0,1> t, v, lower, upper; |
| 220 | pack(t, target); |
| 221 | pack(v, controls); |
| 222 | pack(lower, _lower, controls.size()); |
| 223 | pack(upper, _upper, controls.size()); |
| 224 | |
| 225 | |
| 226 | matrix<double> QQ(K.nr(),K.nr()), RR(K.nc(),K.nc()); |
| 227 | QQ = 0; |
| 228 | RR = 0; |
| 229 | for (unsigned long c = 0; c < controls.size(); ++c) |
| 230 | { |
| 231 | set_subm(QQ,c*Q.nr(),c*Q.nr(),Q.nr(),Q.nr()) = diagm(Q); |
| 232 | set_subm(RR,c*R.nr(),c*R.nr(),R.nr(),R.nr()) = diagm(R); |
| 233 | } |
| 234 | |
| 235 | matrix<double> m1 = trans(K)*QQ*K+RR; |
| 236 | matrix<double> m2 = trans(K)*QQ*(M-t); |
| 237 | |